dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Thomas Gubler 2014-12-05 09:34:22 +01:00
parent 0f1a7e7b5b 6bd586c837
commit 92135d155d
181 changed files with 5786 additions and 3598 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
Debug/NuttX
View File

@ -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
View File

@ -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
Documentation/versionfilter.sh Normal file → Executable file
View File

2
NuttX

@ -1 +1 @@
Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984
Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c

8
ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File

@ -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
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
View File

@ -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

2
ROMFS/px4fmu_common/init.d/10016_3dr_iris
View File

@ -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

3
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
View File

@ -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
ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
View File

@ -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
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
View File

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

4
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
View File

@ -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
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
View File

@ -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

4
ROMFS/px4fmu_common/init.d/11001_hexa_cox
View File

@ -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
ROMFS/px4fmu_common/init.d/12001_octo_cox
View File

@ -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
ROMFS/px4fmu_common/init.d/3030_io_camflyer
View File

@ -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

4
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -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

6
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -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
ROMFS/px4fmu_common/init.d/3033_wingwing
View File

@ -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
ROMFS/px4fmu_common/init.d/3034_fx79
View File

@ -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
ROMFS/px4fmu_common/init.d/3035_viper
View File

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

8
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
View File

@ -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
ROMFS/px4fmu_common/init.d/4001_quad_x
View File

@ -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
ROMFS/px4fmu_common/init.d/4008_ardrone
View File

@ -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
#

4
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -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

4
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -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
ROMFS/px4fmu_common/init.d/4012_quad_x_can
View File

@ -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

2
ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
View File

@ -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
ROMFS/px4fmu_common/init.d/5001_quad_+
View File

@ -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

4
ROMFS/px4fmu_common/init.d/6001_hexa_x
View File

@ -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

4
ROMFS/px4fmu_common/init.d/7001_hexa_+
View File

@ -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
ROMFS/px4fmu_common/init.d/8001_octo_x
View File

@ -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
ROMFS/px4fmu_common/init.d/9001_octo_+
View File

@ -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

5
ROMFS/px4fmu_common/init.d/rc.autostart
View File

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

6
ROMFS/px4fmu_common/init.d/rc.interface
View File

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

10
ROMFS/px4fmu_common/init.d/rc.jig
View File

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

7
ROMFS/px4fmu_common/init.d/rc.sensors
View File

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

30
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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
#
@ -312,7 +301,6 @@ then
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
then
echo "[init] Use PX4IO PWM as primary output"
if px4io start
then
echo "[init] PX4IO started"
@ -325,7 +313,6 @@ then
if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
then
echo "[init] Use FMU as primary output"
if fmu mode_$FMU_MODE
then
echo "[init] FMU mode_$FMU_MODE started"
@ -349,7 +336,6 @@ then
if [ $OUTPUT_MODE == mkblctrl ]
then
echo "[init] Use MKBLCTRL as primary output"
set MKBLCTRL_ARG ""
if [ $MKBLCTRL_MODE == x ]
then
@ -372,7 +358,6 @@ then
if [ $OUTPUT_MODE == hil ]
then
echo "[init] Use HIL as primary output"
if hil mode_port2_pwm8
then
echo "[init] HIL output started"
@ -391,7 +376,6 @@ then
then
if px4io start
then
echo "[init] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[init] ERROR: PX4IO start failed"
@ -424,9 +408,6 @@ then
fi
fi
#
# MAVLink
#
if [ $MAVLINK_FLAGS == default ]
then
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
@ -454,15 +435,10 @@ then
# Sensors, Logging, GPS
#
sh /etc/init.d/rc.sensors
#
# Start logging in all modes, including HIL
#
sh /etc/init.d/rc.logging
if [ $GPS == yes ]
then
echo "[init] Start GPS"
if [ $GPS_FAKE == yes ]
then
echo "[init] Faking GPS"

0
Tools/px_generate_xml.sh Normal file → Executable file
View File

5
Tools/px_mkfw.py
View File

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

0
Tools/px_update_wiki.sh Normal file → Executable file
View File

2
Tools/px_uploader.py
View File

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

6
Tools/upload.sh
View File

@ -10,17 +10,17 @@ SYSTYPE=`uname -s`
#
# XXX The uploader should be smarter than this.
#
if [ $SYSTYPE=Darwin ];
if [ $SYSTYPE = "Darwin" ];
then
SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
fi
if [ $SYSTYPE=Linux ];
if [ $SYSTYPE = "Linux" ];
then
SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*"
fi
if [ $SYSTYPE="" ];
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

5
makefiles/config_px4fmu-v1_default.mk
View File

@ -24,6 +24,8 @@ MODULES += drivers/l3gd20
MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
#MODULES += drivers/ll40ls
MODULES += drivers/trone
#MODULES += drivers/mb12xx
MODULES += drivers/gps
MODULES += drivers/hil
@ -132,6 +134,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.
#

25
makefiles/config_px4fmu-v2_default.mk
View File

@ -27,27 +27,22 @@ MODULES += drivers/l3gd20
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/sf0x
# MODULES += drivers/sf0x
MODULES += drivers/ll40ls
# MODULES += drivers/trone
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
# MODULES += drivers/blinkm
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
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 +56,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 +75,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 +92,6 @@ MODULES += modules/mc_pos_control
#
MODULES += modules/sdlog2
#
# Unit tests
#
#MODULES += modules/unit_test
#MODULES += modules/commander/commander_tests
#
# Library modules
#
@ -139,7 +125,7 @@ MODULES += modules/bottle_drop
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
MODULES += examples/px4_simple_app
#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
@ -156,6 +142,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.
#

7
makefiles/config_px4fmu-v2_test.mk
View File

@ -49,6 +49,13 @@ 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
#

9
makefiles/firmware.mk
View File

@ -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,$<,$@)

6
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -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

2
nuttx-configs/px4fmu-v1/nsh/Make.defs
View File

@ -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 \

2
nuttx-configs/px4fmu-v2/nsh/Make.defs
View File

@ -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 \

3
nuttx-configs/px4fmu-v2/scripts/ld.script
View File

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

2
nuttx-configs/px4io-v1/nsh/Make.defs
View File

@ -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

6
nuttx-configs/px4io-v2/nsh/Make.defs
View File

@ -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

12
src/drivers/airspeed/airspeed.cpp
View File

@ -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;
@ -159,13 +159,15 @@ out:
int
Airspeed::probe()
{
/* on initial power up the device needs more than one retry
for detection. Once it is running then retries aren't
needed
/* on initial power up the device may need more than one retry
for detection. Once it is running the number of retries can
be reduced
*/
_retries = 4;
int ret = measure();
_retries = 0;
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}

36
src/drivers/ardrone_interface/ardrone_interface.c
View File

@ -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);

2
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -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;

2
src/drivers/boards/aerocore/module.mk
View File

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

2
src/drivers/boards/px4fmu-v1/module.mk
View File

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

2
src/drivers/boards/px4fmu-v2/module.mk
View File

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

2
src/drivers/boards/px4io-v1/module.mk
View File

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

2
src/drivers/boards/px4io-v2/module.mk
View File

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

3
src/drivers/device/device.h
View File

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

31
src/drivers/drv_pwm_output.h
View File

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

9
src/drivers/frsky_telemetry/frsky_telemetry.c
View File

@ -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);

1
src/drivers/gps/gps.cpp
View File

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

2
src/drivers/hil/hil.cpp
View File

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

39
src/drivers/hmc5883/hmc5883.cpp
View File

@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr;
void start(int bus, enum Rotation rotation);
void test(int bus);
void reset(int bus);
void info(int bus);
int info(int bus);
int calibrate(int bus);
void usage();
@ -1595,17 +1595,23 @@ reset(int bus)
/**
* Print a little info about the driver.
*/
void
int
info(int bus)
{
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr)
errx(1, "driver not running");
int ret = 1;
printf("state @ %p\n", g_dev);
g_dev->print_info();
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
warnx("not running on bus %d", bus);
} else {
exit(0);
warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
g_dev->print_info();
ret = 0;
}
return ret;
}
void
@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[])
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
hmc5883::info(bus);
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
if (bus == -1) {
int ret = 0;
if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
ret = 1;
}
if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
ret = 1;
}
exit(ret);
} else {
exit(hmc5883::info(bus));
}
}
/*
* Autocalibrate the scaling

2
src/drivers/hmc5883/module.mk
View File

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

8
src/drivers/hott/comms.cpp
View File

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

6
src/drivers/hott/hott_sensors/hott_sensors.cpp
View File

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

6
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
View File

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

2
src/drivers/l3gd20/module.mk
View File

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

2
src/drivers/lsm303d/module.mk
View File

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

2
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

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

2
src/drivers/mpu6000/module.mk
View File

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

353
src/drivers/px4flow/px4flow.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 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
@ -73,15 +73,13 @@
#include <board_config.h>
/* Configuration Constants */
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@ -92,28 +90,42 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
//struct i2c_frame
//{
// uint16_t frame_count;
// int16_t pixel_flow_x_sum;
// int16_t pixel_flow_y_sum;
// int16_t flow_comp_m_x;
// int16_t flow_comp_m_y;
// int16_t qual;
// int16_t gyro_x_rate;
// int16_t gyro_y_rate;
// int16_t gyro_z_rate;
// uint8_t gyro_range;
// uint8_t sonar_timestamp;
// int16_t ground_distance;
//};
//
//struct i2c_frame f;
struct i2c_frame {
uint16_t frame_count;
int16_t pixel_flow_x_sum;
int16_t pixel_flow_y_sum;
int16_t flow_comp_m_x;
int16_t flow_comp_m_y;
int16_t qual;
int16_t gyro_x_rate;
int16_t gyro_y_rate;
int16_t gyro_z_rate;
uint8_t gyro_range;
uint8_t sonar_timestamp;
int16_t ground_distance;
};
struct i2c_frame f;
class PX4FLOW : public device::I2C
typedef struct i2c_integral_frame {
uint16_t frame_count_since_last_readout;
int16_t pixel_flow_x_integral;
int16_t pixel_flow_y_integral;
int16_t gyro_x_rate_integral;
int16_t gyro_y_rate_integral;
int16_t gyro_z_rate_integral;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t ground_distance;
int16_t gyro_temperature;
uint8_t qual;
} __attribute__((packed));
struct i2c_integral_frame f_integral;
class PX4FLOW: public device::I2C
{
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
@ -122,8 +134,8 @@ public:
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
@ -144,42 +156,41 @@ private:
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
@ -189,7 +200,7 @@ private:
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
PX4FLOW::PX4FLOW(int bus, int address) :
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
@ -200,7 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@ -228,21 +239,12 @@ PX4FLOW::init()
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(struct optical_flow_s));
_reports = new RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr) {
goto out;
}
/* get a publish handle on the px4flow topic */
struct optical_flow_s zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0) {
debug("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break;
}
/* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
@ -442,8 +441,7 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d");
debug("i2c::transfer returned %d", ret);
return ret;
}
@ -455,52 +453,96 @@ PX4FLOW::measure()
int
PX4FLOW::collect()
{
int ret = -EIO;
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t val[47] = { 0 };
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
}
if (ret < 0) {
log("error reading from sensor: %d", ret);
debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
// f.flow_comp_m_x= val[7] << 8 | val[6];
// f.flow_comp_m_y= val[9] << 8 | val[8];
// f.qual= val[11] << 8 | val[10];
// f.gyro_x_rate= val[13] << 8 | val[12];
// f.gyro_y_rate= val[15] << 8 | val[14];
// f.gyro_z_rate= val[17] << 8 | val[16];
// f.gyro_range= val[18];
// f.sonar_timestamp= val[19];
// f.ground_distance= val[21] << 8 | val[20];
if (PX4FLOW_REG == 0) {
f.frame_count = val[1] << 8 | val[0];
f.pixel_flow_x_sum = val[3] << 8 | val[2];
f.pixel_flow_y_sum = val[5] << 8 | val[4];
f.flow_comp_m_x = val[7] << 8 | val[6];
f.flow_comp_m_y = val[9] << 8 | val[8];
f.qual = val[11] << 8 | val[10];
f.gyro_x_rate = val[13] << 8 | val[12];
f.gyro_y_rate = val[15] << 8 | val[14];
f.gyro_z_rate = val[17] << 8 | val[16];
f.gyro_range = val[18];
f.sonar_timestamp = val[19];
f.ground_distance = val[21] << 8 | val[20];
f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
f_integral.integration_timespan = val[37] << 24 | val[36] << 16
| val[35] << 8 | val[34];
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
| val[39] << 8 | val[38];
f_integral.ground_distance = val[43] << 8 | val[42];
f_integral.gyro_temperature = val[45] << 8 | val[44];
f_integral.qual = val[46];
}
if (PX4FLOW_REG == 0x16) {
f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
f_integral.ground_distance = val[21] << 8 | val[20];
f_integral.gyro_temperature = val[23] << 8 | val[22];
f_integral.qual = val[24];
}
int16_t flowcx = val[7] << 8 | val[6];
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
struct optical_flow_s report;
report.flow_comp_x_m = float(flowcx) / 1000.0f;
report.flow_comp_y_m = float(flowcy) / 1000.0f;
report.flow_raw_x = val[3] << 8 | val[2];
report.flow_raw_y = val[5] << 8 | val[4];
report.ground_distance_m = float(gdist) / 1000.0f;
report.quality = val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */
if (_reports->force(&report)) {
@ -560,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
void
PX4FLOW::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
debug("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* perform collection */
if (OK != collect()) {
debug("collection error");
/* restart the measurement state machine */
start();
return;
}
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
_measure_ticks);
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
}
void
@ -649,14 +662,41 @@ start()
}
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
#ifdef PX4_I2C_BUS_ESC
delete g_dev;
/* try 2nd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
#endif
delete g_dev;
/* try 3rd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
#ifdef PX4_I2C_BUS_ESC
}
#endif
}
/* set the poll rate to default, starts automatic data collection */
@ -685,7 +725,8 @@ fail:
/**
* Stop the driver
*/
void stop()
void
stop()
{
if (g_dev != nullptr) {
delete g_dev;
@ -716,6 +757,7 @@ test()
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@ -725,18 +767,18 @@ test()
}
warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
/* start the sensor polling at 10Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
errx(1, "failed to set 10Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
for (unsigned i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@ -756,9 +798,22 @@ test()
}
warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
warnx("framecount_total: %u", f.frame_count);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
warnx("ground_distance: %0.2f m",
(double) f_integral.ground_distance / 1000);
warnx("time since last sonar update [us]: %i",
f_integral.time_since_last_sonar_update);
warnx("quality integration average : %i", f_integral.qual);
warnx("quality : %i", f.qual);
}

2
src/drivers/px4fmu/module.mk
View File

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

2
src/drivers/px4io/module.mk
View File

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

198
src/drivers/px4io/px4io.cpp
View File

@ -817,6 +817,11 @@ PX4IO::init()
}
/* set safety to off if circuit breaker enabled */
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
@ -1155,52 +1160,54 @@ PX4IO::io_set_arming_state()
actuator_armed_s armed; ///< system armed state
vehicle_control_mode_s control_mode; ///< vehicle_control_mode
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (have_armed == OK) {
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
}
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
if (have_control_mode == OK) {
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
}
}
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
@ -1245,41 +1252,42 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
/* ROLL */
param_get(param_find("RC_MAP_ROLL"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 0;
}
/* subtract one from 1-based index - this might be
* a negative number now
*/
ichan -= 1;
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 0;
/* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 1;
}
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 1;
/* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 2;
}
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 2;
/* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 3;
}
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 3;
/* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 4;
}
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 4;
/* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
/*
@ -1656,10 +1664,6 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* data we are going to fetch */
actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
@ -2060,7 +2064,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@ -2070,7 +2074,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@ -2195,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
@ -2214,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
@ -2233,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
@ -2252,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
@ -2310,6 +2315,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
/* control whether override on FMU failure is
immediate or waits for override threshold on mode
switch */
if (arg == 0) {
/* clear override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
} else {
/* set override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@ -2571,6 +2589,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_RC_CONFIG: {
/* enable setting of RC configuration without relying
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
/* fail with error */
return -E2BIG;
}
/* copy values to registers in IO */
uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min;
regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim;
regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max;
regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz;
regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
if (config->rc_reverse) {
regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
}
ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
break;
}
case PWM_SERVO_SET_OVERRIDE_OK:
/* set the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
break;
case PWM_SERVO_CLEAR_OVERRIDE_OK:
/* clear the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
break;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);

14
src/drivers/rgbled/rgbled.cpp
View File

@ -121,7 +121,7 @@ private:
/* for now, we only support one RGBLED */
namespace
{
RGBLED *g_rgbled;
RGBLED *g_rgbled = nullptr;
}
void rgbled_usage();
@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[])
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");

2
src/drivers/stm32/adc/module.mk
View File

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

2
src/drivers/stm32/module.mk
View File

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

2
src/drivers/stm32/tone_alarm/module.mk
View File

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

44
src/drivers/trone/module.mk Normal file
View File

@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the TeraRanger One range finder driver
#
MODULE_COMMAND = trone
SRCS = trone.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

913
src/drivers/trone/trone.cpp Normal file
View File

@ -0,0 +1,913 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file trone.cpp
* @author Luis Rodrigues
*
* Driver for the TeraRanger One range finders connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <board_config.h>
/* Configuration Constants */
#define TRONE_BUS PX4_I2C_BUS_EXPANSION
#define TRONE_BASEADDR 0x30 /* 7-bit address */
#define TRONE_DEVICE_PATH "/dev/trone"
/* TRONE Registers addresses */
#define TRONE_MEASURE_REG 0x00 /* Measure range register */
/* Device limits */
#define TRONE_MIN_DISTANCE (0.20f)
#define TRONE_MAX_DISTANCE (14.00f)
#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class TRONE : public device::I2C
{
public:
TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR);
virtual ~TRONE();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE
* and TRONE_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
static const uint8_t crc_table[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
0xfa, 0xfd, 0xf4, 0xf3
};
uint8_t crc8(uint8_t *p, uint8_t len){
uint16_t i;
uint16_t crc = 0x0;
while (len--) {
i = (crc ^ *p++) & 0xFF;
crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int trone_main(int argc, char *argv[]);
TRONE::TRONE(int bus, int address) :
I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000),
_min_distance(TRONE_MIN_DISTANCE),
_max_distance(TRONE_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
TRONE::~TRONE()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
TRONE::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
TRONE::probe()
{
return measure();
}
void
TRONE::set_minimum_distance(float min)
{
_min_distance = min;
}
void
TRONE::set_maximum_distance(float max)
{
_max_distance = max;
}
float
TRONE::get_minimum_distance()
{
return _min_distance;
}
float
TRONE::get_maximum_distance()
{
return _max_distance;
}
int
TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
TRONE::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(TRONE_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
TRONE::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
const uint8_t cmd = TRONE_MEASURE_REG;
ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
TRONE::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[3] = {0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 3);
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.001f; /* mm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
TRONE::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_RANGEFINDER
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
TRONE::stop()
{
work_cancel(HPWORK, &_work);
}
void
TRONE::cycle_trampoline(void *arg)
{
TRONE *dev = (TRONE *)arg;
dev->cycle();
}
void
TRONE::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&TRONE::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&TRONE::cycle_trampoline,
this,
USEC2TICK(TRONE_CONVERSION_INTERVAL));
}
void
TRONE::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace trone
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
TRONE *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new TRONE(TRONE_BUS);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct range_finder_report report;
ssize_t sz;
int ret;
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 50x and report each value */
for (unsigned i = 0; i < 50; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("valid %u", report.valid);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
trone_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
trone::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
trone::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
trone::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
trone::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
trone::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

4
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
if (vehicle_liftoff || params.debug)
{
/* copy flow */
flow_speed[0] = flow.flow_comp_x_m;
flow_speed[1] = flow.flow_comp_y_m;
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */

14
src/lib/external_lgpl/tecs/tecs.cpp
View File

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

298
src/modules/attitude_estimator_ekf/AttitudeEKF.m Normal file
View File

@ -0,0 +1,298 @@
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
%LQG Postion Estimator and Controller
% Observer:
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
% x[n+1|n] = Ax[n|n] + Bu[n]
%
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $
%
%
% Arguments:
% approx_prediction: if 1 then the exponential map is approximated with a
% first order taylor approximation. has at the moment not a big influence
% (just 1st or 2nd order approximation) we should change it to rodriquez
% approximation.
% use_inertia_matrix: set to true if you have the inertia matrix J for your
% quadrotor
% xa_apo_k: old state vectotr
% zFlag: if sensor measurement is available [gyro, acc, mag]
% dt: dt in s
% z: measurements [gyro, acc, mag]
% q_rotSpeed: process noise gyro
% q_rotAcc: process noise gyro acceleration
% q_acc: process noise acceleration
% q_mag: process noise magnetometer
% r_gyro: measurement noise gyro
% r_accel: measurement noise accel
% r_mag: measurement noise mag
% J: moment of inertia matrix
% Output:
% xa_apo: updated state vectotr
% Pa_apo: updated state covariance matrix
% Rot_matrix: rotation matrix
% eulerAngles: euler angles
% debugOutput: not used
%% model specific parameters
% compute once the inverse of the Inertia
persistent Ji;
if isempty(Ji)
Ji=single(inv(J));
end
%% init
persistent x_apo
if(isempty(x_apo))
gyro_init=single([0;0;0]);
gyro_acc_init=single([0;0;0]);
acc_init=single([0;0;-9.81]);
mag_init=single([1;0;0]);
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
end
persistent P_apo
if(isempty(P_apo))
% P_apo = single(eye(NSTATES) * 1000);
P_apo = single(200*ones(12));
end
debugOutput = single(zeros(4,1));
%% copy the states
wx= x_apo(1); % x body angular rate
wy= x_apo(2); % y body angular rate
wz= x_apo(3); % z body angular rate
wax= x_apo(4); % x body angular acceleration
way= x_apo(5); % y body angular acceleration
waz= x_apo(6); % z body angular acceleration
zex= x_apo(7); % x component gravity vector
zey= x_apo(8); % y component gravity vector
zez= x_apo(9); % z component gravity vector
mux= x_apo(10); % x component magnetic field vector
muy= x_apo(11); % y component magnetic field vector
muz= x_apo(12); % z component magnetic field vector
%% prediction section
% compute the apriori state estimate from the previous aposteriori estimate
%body angular accelerations
if (use_inertia_matrix==1)
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
else
wak =[wax;way;waz];
end
%body angular rates
wk =[wx; wy; wz] + dt*wak;
%derivative of the prediction rotation matrix
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
%prediction of the earth z vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
zek =(O*dt+single(eye(3)))*[zex;zey;zez];
else
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
%precision
end
%prediction of the magnetic vector
if (approx_prediction==1)
%e^(Odt)=I+dt*O+dt^2/2!O^2
% so we do a first order approximation of the exponential map
muk =(O*dt+single(eye(3)))*[mux;muy;muz];
else
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
%precision
end
x_apr=[wk;wak;zek;muk];
% compute the apriori error covariance estimate from the previous
%aposteriori estimate
EZ=[0,zez,-zey;
-zez,0,zex;
zey,-zex,0]';
MA=[0,muz,-muy;
-muz,0,mux;
muy,-mux,0]';
E=single(eye(3));
Z=single(zeros(3));
A_lin=[ Z, E, Z, Z
Z, Z, Z, Z
EZ, Z, O, Z
MA, Z, Z, O];
A_lin=eye(12)+A_lin*dt;
%process covariance matrix
persistent Q
if (isempty(Q))
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
q_rotAcc,q_rotAcc,q_rotAcc,...
q_acc,q_acc,q_acc,...
q_mag,q_mag,q_mag]);
end
P_apr=A_lin*P_apo*A_lin'+Q;
%% update
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0,0,0,0;
% 0,r_gyro,0,0,0,0,0,0,0;
% 0,0,r_gyro,0,0,0,0,0,0;
% 0,0,0,r_accel,0,0,0,0,0;
% 0,0,0,0,r_accel,0,0,0,0;
% 0,0,0,0,0,r_accel,0,0,0;
% 0,0,0,0,0,0,r_mag,0,0;
% 0,0,0,0,0,0,0,r_mag,0;
% 0,0,0,0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
%observation matrix
%[zw;ze;zmk];
H_k=[ E, Z, Z, Z;
Z, Z, E, Z;
Z, Z, Z, E];
y_k=z(1:9)-H_k*x_apr;
%S_k=H_k*P_apr*H_k'+R;
S_k=H_k*P_apr*H_k';
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
K_k=(P_apr*H_k'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k)*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
R=[r_gyro,0,0;
0,r_gyro,0;
0,0,r_gyro];
R_v=[r_gyro,r_gyro,r_gyro];
%observation matrix
H_k=[ E, Z, Z, Z];
y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_accel,0,0;
% 0,0,0,0,r_accel,0;
% 0,0,0,0,0,r_accel];
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, E, Z];
y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
% R=[r_gyro,0,0,0,0,0;
% 0,r_gyro,0,0,0,0;
% 0,0,r_gyro,0,0,0;
% 0,0,0,r_mag,0,0;
% 0,0,0,0,r_mag,0;
% 0,0,0,0,0,r_mag];
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
%observation matrix
H_k=[ E, Z, Z, Z;
Z, Z, Z, E];
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
x_apo=x_apr+K_k*y_k;
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
else
x_apo=x_apr;
P_apo=P_apr;
end
end
end
end
%% euler anglels extraction
z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
m_n_b = x_apo(10:12)./norm(x_apo(10:12));
y_n_b=cross(z_n_b,m_n_b);
y_n_b=y_n_b./norm(y_n_b);
x_n_b=(cross(y_n_b,z_n_b));
x_n_b=x_n_b./norm(x_n_b);
xa_apo=x_apo;
Pa_apo=P_apo;
% rotation matrix from earth to body system
Rot_matrix=[x_n_b,y_n_b,z_n_b];
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
theta=-asin(Rot_matrix(1,3));
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
eulerAngles=[phi;theta;psi];

502
src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj Normal file
View File

@ -0,0 +1,502 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
<profile key="profile.mex">
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
<param.RanInstrumentedMex>false</param.RanInstrumentedMex>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks>true</param.ResponsivenessChecks>
<param.ExtrinsicCalls>true</param.ExtrinsicCalls>
<param.IntegrityChecks>true</param.IntegrityChecks>
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
<param.EnableVariableSizing>true</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>200000</param.StackUsageMax>
<param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>false</param.MATLABSourceComments>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.EnableDebugging>false</param.EnableDebugging>
<param.GenerateReport>true</param.GenerateReport>
<param.LaunchReport>false</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.EchoExpressions>true</param.EchoExpressions>
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
<unset>
<param.MergeInstrumentationResults />
<param.BuiltInstrumentedMex />
<param.RanInstrumentedMex />
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.BuildFolder />
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks />
<param.ExtrinsicCalls />
<param.IntegrityChecks />
<param.SaturateOnIntegerOverflow />
<param.GlobalDataSyncMethod />
<param.EnableVariableSizing />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.StackUsageMax />
<param.FilePartitionMethod />
<param.GenerateComments />
<param.MATLABSourceComments />
<param.ReservedNameArray />
<param.EnableScreener />
<param.EnableDebugging />
<param.GenerateReport />
<param.LaunchReport />
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes />
<param.mex.GenCodeOnly />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.TargetLang />
<param.EchoExpressions />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.ConstantInputs />
</unset>
</profile>
<profile key="profile.c">
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
<param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
<param.SearchPaths />
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.PurelyIntegerCode>false</param.PurelyIntegerCode>
<param.SupportNonFinite>false</param.SupportNonFinite>
<param.EnableVariableSizing>false</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>4000</param.StackUsageMax>
<param.MultiInstanceCode>false</param.MultiInstanceCode>
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>true</param.MATLABSourceComments>
<param.MATLABFcnDesc>false</param.MATLABFcnDesc>
<param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
<param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
<param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
<param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
<param.MaxIdLength>31</param.MaxIdLength>
<param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
<param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
<param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
<param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
<param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
<param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
<param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
<param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.Verbose>false</param.Verbose>
<param.GenerateReport>true</param.GenerateReport>
<param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
<param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
<param.LaunchReport>true</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
<param.SameHardware>true</param.SameHardware>
<param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
<param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
<var.instance.enabled.Production>true</var.instance.enabled.Production>
<param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
<param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
<param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
<param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
<param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
<param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
<param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
<param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
<param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
<param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
<param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
<param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
<param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
<param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
<param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
<param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
<param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
<var.instance.enabled.Target>false</var.instance.enabled.Target>
<param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
<param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
<param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
<param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
<param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
<param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
<param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
<param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
<param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
<param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
<param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
<param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
<param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
<param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
<param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
<param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
<param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile>true</param.GenerateMakefile>
<param.BuildToolEnable>false</param.BuildToolEnable>
<param.MakeCommand>make_rtw</param.MakeCommand>
<param.TemplateMakefile>default_tmf</param.TemplateMakefile>
<param.BuildToolConfiguration />
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.PassStructByReference>false</param.PassStructByReference>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<unset>
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.SearchPaths />
<param.SaturateOnIntegerOverflow />
<param.PurelyIntegerCode />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.MultiInstanceCode />
<param.GenerateComments />
<param.MATLABFcnDesc />
<param.DataTypeReplacement />
<param.ConvertIfToSwitch />
<param.PreserveExternInFcnDecls />
<param.ParenthesesLevel />
<param.MaxIdLength />
<param.CustomSymbolStrGlobalVar />
<param.CustomSymbolStrType />
<param.CustomSymbolStrField />
<param.CustomSymbolStrFcn />
<param.CustomSymbolStrTmpVar />
<param.CustomSymbolStrMacro />
<param.CustomSymbolStrEMXArray />
<param.CustomSymbolStrEMXArrayFcn />
<param.ReservedNameArray />
<param.EnableScreener />
<param.Verbose />
<param.GenerateReport />
<param.GenerateCodeMetricsReport />
<param.GenerateCodeReplacementReport />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.SameHardware />
<var.instance.enabled.Production />
<param.HardwareSizeChar.Production />
<param.HardwareSizeShort.Production />
<param.HardwareSizeInt.Production />
<param.HardwareSizeLong.Production />
<param.HardwareSizeLongLong.Production />
<param.HardwareSizeFloat.Production />
<param.HardwareSizeDouble.Production />
<param.HardwareSizeWord.Production />
<param.HardwareSizePointer.Production />
<param.HardwareEndianness.Production />
<param.HardwareLongLongMode.Production />
<param.HardwareDivisionRounding.Production />
<var.instance.enabled.Target />
<param.HardwareSizeChar.Target />
<param.HardwareSizeShort.Target />
<param.HardwareSizeInt.Target />
<param.HardwareSizeLongLong.Target />
<param.HardwareSizeFloat.Target />
<param.HardwareSizeDouble.Target />
<param.HardwareEndianness.Target />
<param.HardwareAtomicFloatSize.Target />
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.IncludeTerminateFcn />
<param.TargetLang />
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile />
<param.BuildToolEnable />
<param.MakeCommand />
<param.TemplateMakefile />
<param.BuildToolConfiguration />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.UseECoderFeatures />
</unset>
</profile>
<param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
<param.version>R2012a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
<param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
<param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
<param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
<param.SILDebugging>false</param.SILDebugging>
<param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
<param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
<param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
<param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
<param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
<param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
<param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
<param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
<param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
<param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
<param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
<param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
<param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
<param.UsePreconditions>false</param.UsePreconditions>
<param.FeatureFlags />
<param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
<param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength>16</param.DefaultWordLength>
<param.DefaultFractionLength>4</param.DefaultFractionLength>
<param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
<param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
<param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
<param.LogAllIOValues>false</param.LogAllIOValues>
<param.LogHistogram>false</param.LogHistogram>
<param.ShowCoverage>true</param.ShowCoverage>
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
<param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
<unset>
<param.outputfile />
<param.version />
<param.HasECoderFeatures />
<param.CallGeneratedCodeFromTest />
<param.VerificationMode />
<param.SILDebugging />
<param.AutoInferUseVariableSize />
<param.AutoInferUseUnboundedSize />
<param.AutoInferVariableSizeThreshold />
<param.AutoInferUnboundedSizeThreshold />
<param.mex.outputfile />
<param.grt.outputfile />
<param.FixedPointTypeProposalMode />
<param.DefaultProposedFixedPointType />
<param.MinMaxSafetyMargin />
<param.OptimizeWholeNumbers />
<param.LaunchInstrumentationReport />
<param.OpenInstrumentationReportInBrowser />
<param.CreatePrintableInstrumentationReport />
<param.EnableAutoExtrinsicCalls />
<param.UsePreconditions />
<param.FeatureFlags />
<param.FixedPointMode />
<param.AutoScaleLoopIndexVariables />
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength />
<param.DefaultFractionLength />
<param.FixedPointSafetyMargin />
<param.FixedPointFimath />
<param.FixedPointTypeSource />
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly />
<param.LogAllIOValues />
<param.LogHistogram />
<param.ShowCoverage />
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.GeneratedFixedPointFileSuffix />
<param.DefaultFixedPointSignedness />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
<Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
<Input Name="approx_prediction">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="use_inertia_matrix">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="zFlag">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>3 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="dt">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="z">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>9 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotSpeed">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotAcc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_acc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_gyro">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_accel">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="J">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>3 x 3</Size>
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.testbench>
<file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
</fileset.testbench>
<fileset.package />
<build-deliverables>
<file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
</build-deliverables>
<workflow />
<matlab>
<root>/opt/matlab/r2013b</root>
<toolboxes>
<toolbox name="fixedpoint" />
</toolboxes>
</matlab>
<platform>
<unix>true</unix>
<mac>false</mac>
<windows>false</windows>
<win2k>false</win2k>
<winxp>false</winxp>
<vista>false</vista>
<linux>true</linux>
<solaris>false</solaris>
<osver>3.16.1-1-ARCH</osver>
<os32>false</os32>
<os64>true</os64>
<arch>glnxa64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>

37
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -38,6 +38,7 @@
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
@ -74,8 +75,7 @@
#ifdef __cplusplus
extern "C" {
#endif
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
#include "codegen/AttitudeEKF.h"
#include "attitude_estimator_ekf_params.h"
#ifdef __cplusplus
}
@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
14000,
7200,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
@ -206,14 +206,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);
float debugOutput[4] = { 0.0f };
int overloadcounter = 19;
/* Initialize filter */
attitudeKalmanfilter_initialize();
AttitudeEKF_initialize();
/* store start time to guard against too slow update rates */
uint64_t last_run = hrt_absolute_time();
@ -277,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* keep track of sensor updates */
uint64_t sensor_last_timestamp[3] = {0, 0, 0};
struct attitude_estimator_ekf_params ekf_params;
struct attitude_estimator_ekf_params ekf_params = { 0 };
struct attitude_estimator_ekf_param_handles ekf_param_handles;
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
/* initialize parameter handles */
parameters_init(&ekf_param_handles);
@ -508,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
continue;
}
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* Call the estimator */
AttitudeEKF(false, // approx_prediction
(unsigned char)ekf_params.use_moment_inertia,
update_vect,
dt,
z_k,
ekf_params.q[0], // q_rotSpeed,
ekf_params.q[1], // q_rotAcc
ekf_params.q[2], // q_acc
ekf_params.q[3], // q_mag
ekf_params.r[0], // r_gyro
ekf_params.r[1], // r_accel
ekf_params.r[2], // r_mag
ekf_params.moment_inertia_J,
x_aposteriori,
P_aposteriori,
Rot_matrix,
euler,
debugOutput);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {

106
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -44,28 +44,96 @@
/* Extended Kalman Filter covariances */
/* gyro process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/* gyro offsets process noise */
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
/* gyro measurement noise */
/**
* Body angular rate process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
/**
* Body angular acceleration process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
/**
* Acceleration process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
/**
* Magnet field vector process noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
/**
* Gyro measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
/* accel measurement noise */
/**
* Accel measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
/* mag measurement noise */
/**
* Mag measurement noise
*
* @group attitude_ekf
*/
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
/* offset estimation - UNUSED */
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
/* magnetic declination, in degrees */
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
/**
* Moment of inertia matrix diagonal entry (1, 1)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
/**
* Moment of inertia matrix diagonal entry (2, 2)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
/**
* Moment of inertia matrix diagonal entry (3, 3)
*
* @group attitude_ekf
* @unit kg*m^2
*/
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
/**
* Moment of inertia enabled in estimator
*
* If set to != 0 the moment of inertia will be used in the estimator
*
* @group attitude_ekf
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(ATT_J_EN, 0);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
/* PID parameters */
@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->q1 = param_find("EKF_ATT_V3_Q1");
h->q2 = param_find("EKF_ATT_V3_Q2");
h->q3 = param_find("EKF_ATT_V3_Q3");
h->q4 = param_find("EKF_ATT_V3_Q4");
h->r0 = param_find("EKF_ATT_V4_R0");
h->r1 = param_find("EKF_ATT_V4_R1");
h->r2 = param_find("EKF_ATT_V4_R2");
h->r3 = param_find("EKF_ATT_V4_R3");
h->mag_decl = param_find("ATT_MAG_DECL");
h->acc_comp = param_find("ATT_ACC_COMP");
h->moment_inertia_J[0] = param_find("ATT_J11");
h->moment_inertia_J[1] = param_find("ATT_J22");
h->moment_inertia_J[2] = param_find("ATT_J33");
h->use_moment_inertia = param_find("ATT_J_EN");
return OK;
}
@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->q1, &(p->q[1]));
param_get(h->q2, &(p->q[2]));
param_get(h->q3, &(p->q[3]));
param_get(h->q4, &(p->q[4]));
param_get(h->r0, &(p->r[0]));
param_get(h->r1, &(p->r[1]));
param_get(h->r2, &(p->r[2]));
param_get(h->r3, &(p->r[3]));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI_F / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
for (int i = 0; i < 3; i++) {
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
}
param_get(h->use_moment_inertia, &(p->use_moment_inertia));
return OK;
}

12
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
View File

@ -42,8 +42,10 @@
#include <systemlib/param/param.h>
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
float r[3];
float q[4];
float moment_inertia_J[9];
int32_t use_moment_inertia;
float roll_off;
float pitch_off;
float yaw_off;
@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3;
param_t q0, q1, q2, q3, q4;
param_t r0, r1, r2;
param_t q0, q1, q2, q3;
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
param_t use_moment_inertia;
param_t mag_decl;
param_t acc_comp;
};

1456
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c Normal file
View File

File diff suppressed because it is too large Load Diff

26
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h Normal file
View File

@ -0,0 +1,26 @@
/*
* AttitudeEKF.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_H__
#define __ATTITUDEEKF_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "AttitudeEKF_types.h"
/* Function Declarations */
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
extern void AttitudeEKF_initialize(void);
extern void AttitudeEKF_terminate(void);
#endif
/* End of code generation (AttitudeEKF.h) */

17
src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h Normal file
View File

@ -0,0 +1,17 @@
/*
* AttitudeEKF_types.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_TYPES_H__
#define __ATTITUDEEKF_TYPES_H__
/* Include files */
#include "rtwtypes.h"
#endif
/* End of code generation (AttitudeEKF_types.h) */

1148
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
View File

File diff suppressed because it is too large Load Diff

34
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
View File

@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter.h
*
* Code generation for function 'attitudeKalmanfilter'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_H__
#define __ATTITUDEKALMANFILTER_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
#endif
/* End of code generation (attitudeKalmanfilter.h) */

31
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
View File

@ -1,31 +0,0 @@
/*
* attitudeKalmanfilter_initialize.c
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "attitudeKalmanfilter_initialize.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void attitudeKalmanfilter_initialize(void)
{
rt_InitInfAndNaN(8U);
}
/* End of code generation (attitudeKalmanfilter_initialize.c) */

34
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
View File

@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter_initialize.h
*
* Code generation for function 'attitudeKalmanfilter_initialize'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter_initialize(void);
#endif
/* End of code generation (attitudeKalmanfilter_initialize.h) */

31
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
View File

@ -1,31 +0,0 @@
/*
* attitudeKalmanfilter_terminate.c
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "attitudeKalmanfilter_terminate.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
void attitudeKalmanfilter_terminate(void)
{
/* (no terminate code required) */
}
/* End of code generation (attitudeKalmanfilter_terminate.c) */

34
src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
View File

@ -1,34 +0,0 @@
/*
* attitudeKalmanfilter_terminate.h
*
* Code generation for function 'attitudeKalmanfilter_terminate'
*
* C source code generated on: Sat Jan 19 15:25:29 2013
*
*/
#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
#define __ATTITUDEKALMANFILTER_TERMINATE_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rt_defines.h"
#include "rt_nonfinite.h"
#include "rtwtypes.h"
#include "attitudeKalmanfilter_types.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
extern void attitudeKalmanfilter_terminate(void);
#endif
/* End of code generation (attitudeKalmanfilter_terminate.h) */

Some files were not shown because too many files have changed in this diff Show More