fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged PX4Flow driver changes

Browse Source
This commit is contained in:
Lorenz Meier 2014-11-26 07:43:19 +01:00
commit cbcd1ea1d1
155 changed files with 2152 additions and 765 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 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c
Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097

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

25
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
#
@ -424,9 +413,6 @@ then
fi
fi
#
# MAVLink
#
if [ $MAVLINK_FLAGS == default ]
then
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
@ -454,15 +440,10 @@ then
# Sensors, Logging, GPS
#
sh /etc/init.d/rc.sensors
#
# Start logging in all modes, including HIL
#
sh /etc/init.d/rc.logging
if [ $GPS == yes ]
then
echo "[init] Start GPS"
if [ $GPS_FAKE == yes ]
then
echo "[init] Faking GPS"

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

12
Tools/px_uploader.py
View File

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

28
Tools/upload.sh Executable file
View File

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

55
Tools/usb_serialload.py Normal file
View File

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

3
makefiles/config_px4fmu-v1_default.mk
View File

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

20
makefiles/config_px4fmu-v2_default.mk
View File

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

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
mavlink/include/mavlink/v1.0

@ -1 +1 @@
Subproject commit 90383fac84d031aef17989a1497c2473dfa64340
Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30

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

1
src/drivers/boards/px4io-v2/board_config.h
View File

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

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

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

1
src/drivers/boards/px4io-v2/px4iov2_init.c
View File

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

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 */

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

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

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

9
src/drivers/l3gd20/l3gd20.cpp
View File

@ -176,6 +176,7 @@ static const int ERROR = -1;
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
@ -856,7 +857,7 @@ L3GD20::measure()
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
@ -930,6 +931,8 @@ L3GD20::measure()
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@ -938,6 +941,8 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
@ -1091,9 +1096,11 @@ test()
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("temp: \t%d\tC", (int)g_report.temperature);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));

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

14
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
@ -75,7 +75,7 @@
/* Configuration Constants */
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
@ -211,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));
@ -441,7 +441,7 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
debug("i2c::transfer returned %d", ret);
return ret;
}
@ -469,7 +469,7 @@ PX4FLOW::collect()
}
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;
@ -603,12 +603,12 @@ void
PX4FLOW::cycle()
{
if (OK != measure()) {
log("measure error");
debug("measure error");
}
/* perform collection */
if (OK != collect()) {
log("collection error");
debug("collection error");
/* restart the measurement state machine */
start();
return;

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

181
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,34 +1252,40 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
/* ROLL */
param_get(param_find("RC_MAP_ROLL"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 0;
}
/* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 1;
}
/* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 2;
}
/* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 3;
}
/* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 4;
}
/* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
@ -1651,10 +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();
@ -2055,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"),
@ -2065,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",
@ -2190,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);
@ -2209,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);
@ -2228,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);
@ -2247,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);
@ -2305,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 */
@ -2566,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);

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

4
src/lib/conversion/rotation.h
View File

@ -74,6 +74,7 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_ROLL_270_YAW_270 = 26,
ROTATION_MAX
};
@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
{ 0, 270, 0 },
{270, 0, 270 }
};
/**

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) {

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

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

21
src/modules/bottle_drop/bottle_drop.cpp
View File

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

4
src/modules/bottle_drop/module.mk
View File

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

50
src/modules/commander/accelerometer_calibration.cpp
View File

@ -83,7 +83,7 @@
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = {
0.0f,
1.0f,
@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) {
res = ERROR;
break;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(3);
continue;
}
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
/* inform user about already handled side */
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(4);
continue;
}
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0;
}
}

25
src/modules/commander/airspeed_calibration.cpp
View File

@ -61,6 +61,15 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
close(diff_pres_sub);
return ERROR;
}
@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)",
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}

184
src/modules/commander/commander.cpp
View File

@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "arm")) {
arm_disarm(true, mavlink_fd, "command line");
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(true, mavlink_fd_local, "command line");
close(mavlink_fd_local);
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
arm_disarm(false, mavlink_fd, "command line");
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
arm_disarm(false, mavlink_fd_local, "command line");
close(mavlink_fd_local);
exit(0);
}
@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[])
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
@ -1407,8 +1412,8 @@ int commander_thread_main(int argc, char *argv[])
last_idle_time = system_load.tasks[0].total_runtime;
/* check if board is connected via USB */
//struct stat statbuf;
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
struct stat statbuf;
on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
/* if battery voltage is getting lower, warn using buzzer, etc. */
@ -1418,7 +1423,7 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
} else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
@ -1543,7 +1548,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC signal regained");
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
status_changed = true;
}
}
@ -1644,8 +1649,9 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp=sp_man.timestamp;
status_changed = true;
}
}
@ -1848,7 +1854,11 @@ int commander_thread_main(int argc, char *argv[])
if (status.failsafe != failsafe_old) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
if (status.failsafe) {
mavlink_log_critical(mavlink_fd, "failsafe mode on");
} else {
mavlink_log_critical(mavlink_fd, "failsafe mode off");
}
failsafe_old = status.failsafe;
}
@ -2196,6 +2206,59 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RCRECOVER:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@ -2208,18 +2271,46 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_DESCEND:
/* TODO: check if this makes sense */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_termination_enabled = true;
break;
case NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@ -2275,73 +2366,6 @@ set_control_mode()
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
}
break;
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTGS:
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_termination_enabled = true;
break;
default:

5
src/modules/commander/gyro_calibration.cpp
View File

@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale = {
0.0f,

2
src/modules/commander/mag_calibration.cpp
View File

@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0;

37
src/modules/commander/state_machine_helper.cpp
View File

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

2
src/modules/controllib/module.mk
View File

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

14
src/modules/dataman/dataman.c
View File

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

9
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

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

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