diff --git a/Debug/NuttX b/Debug/NuttX
index d34e9f5b4b75fd086aefa13d7872dad803a14806..4b9f4b5a1bef759eb2f2e4f1954cbd2da6c3b2a5 100644
--- a/Debug/NuttX
+++ b/Debug/NuttX
@@ -78,7 +78,7 @@ end
################################################################################
define showfiles
- set $task = (struct _TCB *)$arg0
+ set $task = (struct tcb_s *)$arg0
set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file)
printf "%d files\n", $nfiles
set $index = 0
@@ -102,7 +102,7 @@ end
################################################################################
define _showtask_oneline
- set $task = (struct _TCB *)$arg0
+ set $task = (struct tcb_s *)$arg0
printf " %p %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name
end
@@ -139,7 +139,7 @@ end
# Print task registers for a NuttX v7em target with FPU enabled.
#
define _showtaskregs_v7em
- set $task = (struct _TCB *)$arg0
+ set $task = (struct tcb_s *)$arg0
set $regs = (uint32_t *)&($task->xcp.regs[0])
printf " r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5]
@@ -162,7 +162,7 @@ end
define _showsemaphore
printf "count %d ", $arg0->semcount
if $arg0->holder.htcb != 0
- set $_task = (struct _TCB *)$arg0->holder.htcb
+ set $_task = (struct tcb_s *)$arg0->holder.htcb
printf "held by %s", $_task->name
end
printf "\n"
@@ -172,7 +172,7 @@ end
# Print information about a task's stack usage
#
define showtaskstack
- set $task = (struct _TCB *)$arg0
+ set $task = (struct tcb_s *)$arg0
if $task == &g_idletcb
printf "can't measure idle stack\n"
@@ -189,7 +189,7 @@ end
# Print details of a task
#
define showtask
- set $task = (struct _TCB *)$arg0
+ set $task = (struct tcb_s *)$arg0
printf "%p %.2d ", $task, $task->pid
_showtaskstate $task
@@ -204,7 +204,7 @@ define showtask
if $task->task_state != TSTATE_TASK_RUNNING
_showtaskregs_v7em $task
else
- _showcurrentregs_v7em
+ _showtaskregs_v7em $task
end
# XXX print registers here
@@ -247,8 +247,10 @@ define showtasks
_showtasklist &g_pendingtasks
printf "RUNNABLE\n"
_showtasklist &g_readytorun
- printf "WAITING\n"
+ printf "WAITING for Semaphore\n"
_showtasklist &g_waitingforsemaphore
+ printf "WAITING for Signal\n"
+ _showtasklist &g_waitingforsignal
printf "INACTIVE\n"
_showtasklist &g_inactivetasks
end
@@ -257,3 +259,23 @@ document showtasks
. showtasks
. Print a list of all tasks in the system, separated into their respective queues.
end
+
+define my_mem
+
+ set $start = $arg0
+ set $end = $arg1
+ set $cursor = $start
+
+ if $start < $end
+ while $cursor != $end
+ set *$cursor = 0x0000
+ set $cursor = $cursor + 4
+ printf "0x%x of 0x%x\n",$cursor,$end
+ end
+ else
+ while $cursor != $end
+ set *$cursor = 0x0000
+ set $cursor = $cursor - 4
+ end
+ end
+end
\ No newline at end of file
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py
index 7cc21b99f43880afcc4a8d2285f19ebe7d073145..cf86dd668f8641ae7aaee568cea65deef543aa1d 100644
--- a/Debug/Nuttx.py
+++ b/Debug/Nuttx.py
@@ -59,30 +59,42 @@ class NX_register_set(object):
def __init__(self, xcpt_regs):
if xcpt_regs is None:
- self.regs['R0'] = long(gdb.parse_and_eval('$r0'))
- self.regs['R1'] = long(gdb.parse_and_eval('$r1'))
- self.regs['R2'] = long(gdb.parse_and_eval('$r2'))
- self.regs['R3'] = long(gdb.parse_and_eval('$r3'))
- self.regs['R4'] = long(gdb.parse_and_eval('$r4'))
- self.regs['R5'] = long(gdb.parse_and_eval('$r5'))
- self.regs['R6'] = long(gdb.parse_and_eval('$r6'))
- self.regs['R7'] = long(gdb.parse_and_eval('$r7'))
- self.regs['R8'] = long(gdb.parse_and_eval('$r8'))
- self.regs['R9'] = long(gdb.parse_and_eval('$r9'))
- self.regs['R10'] = long(gdb.parse_and_eval('$r10'))
- self.regs['R11'] = long(gdb.parse_and_eval('$r11'))
- self.regs['R12'] = long(gdb.parse_and_eval('$r12'))
- self.regs['R13'] = long(gdb.parse_and_eval('$r13'))
- self.regs['SP'] = long(gdb.parse_and_eval('$sp'))
- self.regs['R14'] = long(gdb.parse_and_eval('$r14'))
- self.regs['LR'] = long(gdb.parse_and_eval('$lr'))
- self.regs['R15'] = long(gdb.parse_and_eval('$r15'))
- self.regs['PC'] = long(gdb.parse_and_eval('$pc'))
- self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr'))
+ self.regs['R0'] = self.mon_reg_call('r0')
+ self.regs['R1'] = self.mon_reg_call('r1')
+ self.regs['R2'] = self.mon_reg_call('r2')
+ self.regs['R3'] = self.mon_reg_call('r3')
+ self.regs['R4'] = self.mon_reg_call('r4')
+ self.regs['R5'] = self.mon_reg_call('r5')
+ self.regs['R6'] = self.mon_reg_call('r6')
+ self.regs['R7'] = self.mon_reg_call('r7')
+ self.regs['R8'] = self.mon_reg_call('r8')
+ self.regs['R9'] = self.mon_reg_call('r9')
+ self.regs['R10'] = self.mon_reg_call('r10')
+ self.regs['R11'] = self.mon_reg_call('r11')
+ self.regs['R12'] = self.mon_reg_call('r12')
+ self.regs['R13'] = self.mon_reg_call('r13')
+ self.regs['SP'] = self.mon_reg_call('sp')
+ self.regs['R14'] = self.mon_reg_call('r14')
+ self.regs['LR'] = self.mon_reg_call('lr')
+ self.regs['R15'] = self.mon_reg_call('r15')
+ self.regs['PC'] = self.mon_reg_call('pc')
+ self.regs['XPSR'] = self.mon_reg_call('xPSR')
else:
for key in self.v7em_regmap.keys():
self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]])
+ def mon_reg_call(self,register):
+ """
+ register is the register as a string e.g. 'pc'
+ return integer containing the value of the register
+ """
+ str_to_eval = "mon reg "+register
+ resp = gdb.execute(str_to_eval,to_string = True)
+ content = resp.split()[-1];
+ try:
+ return int(content,16)
+ except:
+ return 0
@classmethod
def with_xcpt_regs(cls, xcpt_regs):
@@ -172,7 +184,7 @@ class NX_task(object):
self.__dict__['stack_used'] = 0
else:
stack_limit = self._tcb['adj_stack_size']
- for offset in range(0, stack_limit):
+ for offset in range(0, int(stack_limit)):
if stack_base[offset] != 0xff:
break
self.__dict__['stack_used'] = stack_limit - offset
@@ -187,7 +199,7 @@ class NX_task(object):
def state(self):
"""return the name of the task's current state"""
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
- for name,value in statenames.iteritems():
+ for name,value in statenames.items():
if value == self._tcb['task_state']:
return name
return 'UNKNOWN'
@@ -196,16 +208,19 @@ class NX_task(object):
def waiting_for(self):
"""return a description of what the task is waiting for, if it is waiting"""
if self._state_is('TSTATE_WAIT_SEM'):
- waitsem = self._tcb['waitsem'].dereference()
- waitsem_holder = waitsem['holder']
- holder = NX_task.for_tcb(waitsem_holder['htcb'])
- if holder is not None:
- return '{}({})'.format(waitsem.address, holder.name)
- else:
- return '{}(<bad holder>)'.format(waitsem.address)
+ try:
+ waitsem = self._tcb['waitsem'].dereference()
+ waitsem_holder = waitsem['holder']
+ holder = NX_task.for_tcb(waitsem_holder['htcb'])
+ if holder is not None:
+ return '{}({})'.format(waitsem.address, holder.name)
+ else:
+ return '{}(<bad holder>)'.format(waitsem.address)
+ except:
+ return 'EXCEPTION'
if self._state_is('TSTATE_WAIT_SIG'):
return 'signal'
- return None
+ return ""
@property
def is_waiting(self):
@@ -229,7 +244,7 @@ class NX_task(object):
filearray = filelist['fl_files']
result = dict()
for i in range(filearray.type.range()[0],filearray.type.range()[1]):
- inode = long(filearray[i]['f_inode'])
+ inode = int(filearray[i]['f_inode'])
if inode != 0:
result[i] = inode
return result
@@ -253,7 +268,18 @@ class NX_task(object):
def __str__(self):
return "{}:{}".format(self.pid, self.name)
-
+
+ def showoff(self):
+ print("-------")
+ print(self.pid,end = ", ")
+ print(self.name,end = ", ")
+ print(self.state,end = ", ")
+ print(self.waiting_for,end = ", ")
+ print(self.stack_used,end = ", ")
+ print(self._tcb['adj_stack_size'],end = ", ")
+ print(self.file_descriptors)
+ print(self.registers)
+
def __format__(self, format_spec):
return format_spec.format(
pid = self.pid,
@@ -265,7 +291,7 @@ class NX_task(object):
file_descriptors = self.file_descriptors,
registers = self.registers
)
-
+
class NX_show_task (gdb.Command):
"""(NuttX) prints information about a task"""
@@ -285,7 +311,7 @@ class NX_show_task (gdb.Command):
my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
my_fmt += ' R12 {registers[PC]:#010x}\n'
my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
- print format(t, my_fmt)
+ print(format(t, my_fmt))
class NX_show_tasks (gdb.Command):
"""(NuttX) prints a list of tasks"""
@@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command):
def invoke(self, args, from_tty):
tasks = NX_task.tasks()
+ print ('Number of tasks: ' + str(len(tasks)))
for t in tasks:
- print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
+ #t.showoff()
+ print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}'))
NX_show_task()
NX_show_tasks()
@@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command):
def __init__(self):
super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
- struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
- preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
- if preceding_size == 2:
+ struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s')
+ preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof
+ if preceding_size == 2:
self._allocflag = 0x8000
- elif preceding_size == 4:
+ elif preceding_size == 4:
self._allocflag = 0x80000000
- else:
- raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
- self._allocnodesize = struct_mm_allocnode_s.sizeof
+ else:
+ raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size)
+ self._allocnodesize = struct_mm_allocnode_s.sizeof
def _node_allocated(self, allocnode):
if allocnode['preceding'] & self._allocflag:
@@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command):
if region_start >= region_end:
raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start)))
nodecount = region_end - region_start
- print 'heap {} - {}'.format(region_start, region_end)
+ print ('heap {} - {}'.format(region_start, region_end))
cursor = 1
while cursor < nodecount:
allocnode = region_start[cursor]
@@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command):
state = ''
else:
state = '(free)'
- print ' {} {} {}'.format(allocnode.address + self._allocnodesize,
- self._node_size(allocnode), state)
+ print( ' {} {} {}'.format(allocnode.address + self._allocnodesize,
+ self._node_size(allocnode), state))
cursor += self._node_size(allocnode) / self._allocnodesize
def invoke(self, args, from_tty):
@@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command):
nregions = heap['mm_nregions']
region_starts = heap['mm_heapstart']
region_ends = heap['mm_heapend']
- print '{} heap(s)'.format(nregions)
+ print( '{} heap(s)'.format(nregions))
# walk the heaps
for i in range(0, nregions):
self._print_allocations(region_starts[i], region_ends[i])
@@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command):
my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
my_fmt += ' R12 {registers[PC]:#010x}\n'
my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
- print format(registers, my_fmt)
+ print (format(registers, my_fmt))
NX_show_interrupted_thread()
+
+class NX_check_tcb(gdb.Command):
+ """ check the tcb of a task from a address """
+ def __init__(self):
+ super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER)
+
+ def invoke(self,args,sth):
+ tasks = NX_task.tasks()
+ print("tcb int: ",int(args))
+ print(tasks[int(args)]._tcb)
+ a =tasks[int(args)]._tcb['xcp']['regs']
+ print("relevant registers:")
+ for reg in regmap:
+ hex_addr= hex(int(a[regmap[reg]]))
+ eval_string = 'info line *'+str(hex_addr)
+ print(reg,": ",hex_addr,)
+NX_check_tcb()
+
+class NX_tcb(object):
+ def __init__(self):
+ pass
+
+ def is_in(self,arg,list):
+ for i in list:
+ if arg == i:
+ return True;
+ return False
+
+ def find_tcb_list(self,dq_entry_t):
+ tcb_list = []
+ tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+ first_tcb = tcb_ptr.dereference()
+ tcb_list.append(first_tcb);
+ next_tcb = first_tcb['flink'].dereference()
+ while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+ tcb_list.append(next_tcb);
+ old_tcb = next_tcb;
+ next_tcb = old_tcb['flink'].dereference()
+
+ return [t for t in tcb_list if int(t['pid'])<2000]
+
+ def getTCB(self):
+ list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+ tcb_list = [];
+ for l in list_of_listsnames:
+ li = gdb.lookup_global_symbol(l)
+ print(li)
+ cursor = li.value()['head']
+ tcb_list = tcb_list + self.find_tcb_list(cursor)
+
+class NX_check_stack_order(gdb.Command):
+ """ Check the Stack order corresponding to the tasks """
+
+ def __init__(self):
+ super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER)
+
+ def is_in(self,arg,list):
+ for i in list:
+ if arg == i:
+ return True;
+ return False
+
+ def find_tcb_list(self,dq_entry_t):
+ tcb_list = []
+ tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+ first_tcb = tcb_ptr.dereference()
+ tcb_list.append(first_tcb);
+ next_tcb = first_tcb['flink'].dereference()
+ while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+ tcb_list.append(next_tcb);
+ old_tcb = next_tcb;
+ next_tcb = old_tcb['flink'].dereference()
+
+ return [t for t in tcb_list if int(t['pid'])<2000]
+
+ def getTCB(self):
+ list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+ tcb_list = [];
+ for l in list_of_listsnames:
+ li = gdb.lookup_global_symbol(l)
+ cursor = li.value()['head']
+ tcb_list = tcb_list + self.find_tcb_list(cursor)
+ return tcb_list
+
+ def getSPfromTask(self,tcb):
+ regmap = NX_register_set.v7em_regmap
+ a =tcb['xcp']['regs']
+ return int(a[regmap['SP']])
+
+ def find_closest(self,list,val):
+ tmp_list = [abs(i-val) for i in list]
+ tmp_min = min(tmp_list)
+ idx = tmp_list.index(tmp_min)
+ return idx,list[idx]
+
+ def find_next_stack(self,address,_dict_in):
+ add_list = []
+ name_list = []
+ for key in _dict_in.keys():
+ for i in range(3):
+ if _dict_in[key][i] < address:
+ add_list.append(_dict_in[key][i])
+ if i == 2: # the last one is the processes stack pointer
+ name_list.append(self.check_name(key)+"_SP")
+ else:
+ name_list.append(self.check_name(key))
+
+ idx,new_address = self.find_closest(add_list,address)
+ return new_address,name_list[idx]
+
+ def check_name(self,name):
+ if isinstance(name,(list)):
+ name = name[0];
+ idx = name.find("\\")
+ newname = name[:idx]
+
+ return newname
+
+ def invoke(self,args,sth):
+ tcb = self.getTCB();
+ stackadresses={};
+ for t in tcb:
+ p = [];
+ #print(t.name,t._tcb['stack_alloc_ptr'])
+ p.append(int(t['stack_alloc_ptr']))
+ p.append(int(t['adj_stack_ptr']))
+ p.append(self.getSPfromTask(t))
+ stackadresses[str(t['name'])] = p;
+ address = int("0x30000000",0)
+ print("stack address : process")
+ for i in range(len(stackadresses)*3):
+ address,name = self.find_next_stack(address,stackadresses)
+ print(hex(address),": ",name)
+
+NX_check_stack_order()
+
+class NX_run_debug_util(gdb.Command):
+ """ show the registers of a task corresponding to a tcb address"""
+ def __init__(self):
+ super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER)
+
+ def printRegisters(self,task):
+ regmap = NX_register_set.v7em_regmap
+ a =task._tcb['xcp']['regs']
+ print("relevant registers in ",task.name,":")
+ for reg in regmap:
+ hex_addr= hex(int(a[regmap[reg]]))
+ eval_string = 'info line *'+str(hex_addr)
+ print(reg,": ",hex_addr,)
+
+ def getPCfromTask(self,task):
+ regmap = NX_register_set.v7em_regmap
+ a =task._tcb['xcp']['regs']
+ return hex(int(a[regmap['PC']]))
+
+ def invoke(self,args,sth):
+ tasks = NX_task.tasks()
+ if args == '':
+ for t in tasks:
+ self.printRegisters(t)
+ eval_str = "list *"+str(self.getPCfromTask(t))
+ print("this is the location in code where the current threads $pc is:")
+ gdb.execute(eval_str)
+ else:
+ tcb_nr = int(args);
+ print("tcb_nr = ",tcb_nr)
+ t = tasks[tcb_nr]
+ self.printRegisters(t)
+ eval_str = "list *"+str(self.getPCfromTask(t))
+ print("this is the location in code where the current threads $pc is:")
+ gdb.execute(eval_str)
+
+NX_run_debug_util()
+
+
+class NX_search_tcb(gdb.Command):
+ """ shot PID's of all running tasks """
+
+ def __init__(self):
+ super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER)
+
+ def is_in(self,arg,list):
+ for i in list:
+ if arg == i:
+ return True;
+ return False
+
+ def find_tcb_list(self,dq_entry_t):
+ tcb_list = []
+ tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer())
+ first_tcb = tcb_ptr.dereference()
+ tcb_list.append(first_tcb);
+ next_tcb = first_tcb['flink'].dereference()
+ while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]):
+ tcb_list.append(next_tcb);
+ old_tcb = next_tcb;
+ next_tcb = old_tcb['flink'].dereference()
+
+ return [t for t in tcb_list if int(t['pid'])<2000]
+
+ def invoke(self,args,sth):
+ list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks']
+ tasks = [];
+ for l in list_of_listsnames:
+ li = gdb.lookup_global_symbol(l)
+ cursor = li.value()['head']
+ tasks = tasks + self.find_tcb_list(cursor)
+
+ # filter for tasks that are listed twice
+ tasks_filt = {}
+ for t in tasks:
+ pid = int(t['pid']);
+ if not pid in tasks_filt.keys():
+ tasks_filt[pid] = t['name'];
+ print('{num_t} Tasks found:'.format(num_t = len(tasks_filt)))
+ for pid in tasks_filt.keys():
+ print("PID: ",pid," ",tasks_filt[pid])
+
+NX_search_tcb()
+
+
+class NX_my_bt(gdb.Command):
+ """ 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list
+ arg: tcb_address$
+ (can easily be found by typing 'showtask').
+ """
+
+ def __init__(self):
+ super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER)
+
+ def readmem(self,addr):
+ '''
+ read memory at addr and return nr
+ '''
+ str_to_eval = "x/x "+hex(addr)
+ resp = gdb.execute(str_to_eval,to_string = True)
+ idx = resp.find('\t')
+ return int(resp[idx:],16)
+
+ def is_in_bounds(self,val):
+ lower_bound = int("08004000",16)
+ upper_bound = int("080ae0c0",16);
+ #print(lower_bound," ",val," ",upper_bound)
+ if val>lower_bound and val<upper_bound:
+ return True;
+ else:
+ return False;
+ def get_tcb_from_address(self,addr):
+ addr_value = gdb.Value(addr)
+ tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer())
+ return tcb_ptr.dereference()
+
+ def print_instruction_at(self,addr,stack_percentage):
+ gdb.write(str(round(stack_percentage,2))+":")
+ str_to_eval = "info line *"+hex(addr)
+ #gdb.execute(str_to_eval)
+ res = gdb.execute(str_to_eval,to_string = True)
+ # get information from results string:
+ words = res.split()
+ valid = False
+ if words[0] == 'No':
+ #no line info...
+ pass
+ else:
+ valid = True
+ line = int(words[1])
+ idx = words[3].rfind("/"); #find first backslash
+ if idx>0:
+ name = words[3][idx+1:];
+ path = words[3][:idx];
+ else:
+ name = words[3];
+ path = "";
+ block = gdb.block_for_pc(addr)
+ func = block.function
+ if str(func) == "None":
+ func = block.superblock.function
+
+ if valid:
+ print("Line: ",line," in ",path,"/",name,"in ",func)
+ return name,path,line,func
+
+
+
+
+ def invoke(self,args,sth):
+ addr_dec = int(args[2:],16)
+ _tcb = self.get_tcb_from_address(addr_dec)
+ print("found task with PID: ",_tcb["pid"])
+ up_stack = int(_tcb['adj_stack_ptr'])
+ curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer
+ other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer
+ stacksize = int(_tcb['adj_stack_size']) # other stack pointer
+
+ print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack))
+
+ if curr_sp == up_stack:
+ sp = other_sp
+ else:
+ sp = curr_sp;
+
+ while(sp < up_stack):
+ mem = self.readmem(sp)
+ #print(hex(sp)," : ",hex(mem))
+ if self.is_in_bounds(mem):
+ # this is a potential instruction ptr
+ stack_percentage = (up_stack-sp)/stacksize
+ name,path,line,func = self.print_instruction_at(mem,stack_percentage)
+ sp = sp + 4; # jump up one word
+
+NX_my_bt()
diff --git a/Documentation/versionfilter.sh b/Documentation/versionfilter.sh
old mode 100644
new mode 100755
diff --git a/NuttX b/NuttX
index 5ee4b2b2c26bbc35d1669840f0676e8aa383b984..ec6b670f6d1e964700c0b0a50d14db12761e3097 160000
--- a/NuttX
+++ b/NuttX
@@ -1 +1 @@
-Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984
+Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
index d114fe21a2a2e3c1917163624d2ad00b3cb313d6..4d6d350b8a0475128b67f4f3efdef56ab3b218e9 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -1,14 +1,10 @@
#!nsh
#
-# HILStar / X-Plane
-#
-# Lorenz Meier <lm@inf.ethz.ch>
+# HILStar
+# <lorenz@px4.io>
#
sh /etc/init.d/rc.fw_defaults
-echo "X Plane HIL starting.."
-
set HIL yes
-
set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
index b1aa8c00b36205f3a9706e164ee20840be01cc24..c8379e3a1aac2b2c4eb50028c03fcf56240eb392 100644
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -2,7 +2,7 @@
#
# Team Blacksheep Discovery Quadcopter
#
-# Anton Babushkin <anton.babushkin@me.com>, Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
+# Anton Babushkin <anton@px4.io>, Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index 3f47390c150235cb2291572ce11babbc182438cb..0b422de7e2b3ec27edf024b2c3095fb8ef6eb068 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -2,7 +2,7 @@
#
# 3DR Iris Quadcopter
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
index 6179855f6abc9ecbc902a5f8fcb3a37564ddfcdc..a621de7ce88bd0ef49e883d3f7e8d52205dcee0a 100644
--- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
@@ -2,8 +2,7 @@
#
# Steadidrone QU4D
#
-# Thomas Gubler <thomasgubler@gmail.com>
-# Lorenz Meier <lm@inf.ethz.ch>
+# Thomas Gubler <thomas@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
index 7a7a9542c044d768bf46ca51e35786c284984274..1c4f6803b661b70a24b8395abfdc1824568e7d0b 100644
--- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
+++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil
@@ -2,7 +2,7 @@
#
# HIL Quadcopter X
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
index c47500c7ac23a1fa4b8f15754b3b8e0e2c5faa94..0cbdd75bedcb056ad803bca11959f68913d63eed 100644
--- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -2,7 +2,7 @@
#
# HIL Quadcopter +
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
index 4e3e1832623d1d86ad54cf8b8590bb866cab3800..fb440d2fc7dac2ed8da5f8d7f3f14df9d69a2a6b 100644
--- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -2,13 +2,11 @@
#
# HIL Rascal 110 (Flightgear)
#
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
#
sh /etc/init.d/rc.fw_defaults
-echo "HIL Rascal 110 starting.."
-
set HIL yes
set MIXER FMU_AERT
diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
index 941f5664ad7f153b535d3e51cb2f0f406b973f28..00b97d675c96654f8b9585c07874af0fc9db75f6 100644
--- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
@@ -2,7 +2,7 @@
#
# HIL Malolo 1 (Flightgear)
#
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
#
sh /etc/init.d/rc.fw_defaults
diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
index daa04a4de80d0165247f82a0a9cf2cb3705a8e1b..e4d96fbd567759285c3bfcd457bc59ecd7ea8211 100644
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -2,12 +2,12 @@
#
# Generic 10" Hexa coaxial geometry
#
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_cox
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox
index 8703f5f2fe30b644b013d26320735591378efbee..f820251ad3822d816a3ccb6ce286c93cf374ca91 100644
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -2,7 +2,7 @@
#
# Generic 10" Octo coaxial geometry
#
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
index 83c470234c4d4a4c268bd855c1f0b23d22236b37..0f0cb3a89f09f29c6a7256558459c33aa94e8c05 100644
--- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
@@ -3,3 +3,6 @@
sh /etc/init.d/rc.fw_defaults
set MIXER FMU_Q
+# Provide ESC a constant 1000 us pulse while disarmed
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 53c48d8aac140ab49207763ed8845ebdc0700168..a7749cba6eb1df5d4ac16653711a10526ad902e5 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -2,7 +2,7 @@
#
# Phantom FPV Flying Wing
#
-# Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
+# Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.fw_defaults
@@ -21,8 +21,6 @@ then
param set FW_PR_P 0.03
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
- param set FW_P_RMAX_NEG 0
- param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1
param set FW_RR_FF 0.5
param set FW_RR_I 0.02
diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
index 7d0dc5bff3f49f6572fbfda80a4ec18fbdadab20..26c7c95e673231142935f966b870e504e531c4dc 100644
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -2,7 +2,7 @@
#
# Skywalker X5 Flying Wing
#
-# Thomas Gubler <thomasgubler@gmail.com>, Julian Oes <joes@student.ethz.ch>
+# Thomas Gubler <thomas@px4.io>, Julian Oes <julian@px4.io>
#
sh /etc/init.d/rc.fw_defaults
@@ -19,10 +19,6 @@ then
param set FW_PR_I 0
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.03
- param set FW_P_LIM_MAX 45
- param set FW_P_LIM_MIN -45
- param set FW_P_RMAX_NEG 0
- param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1
param set FW_RR_FF 0.3
param set FW_RR_I 0
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index f4dedef15cc4e5b1ff54e4cc8efbc75efd8a4ed4..919eefb4a886cfb857e3ac4244e7e0258ba55848 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -2,7 +2,7 @@
#
# Wing Wing (aka Z-84) Flying Wing
#
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.fw_defaults
diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79
index f4bd18269d41c0001362e87202a3fe42824494a2..4a76ba6ebd0aac6265c4390c092a8fb47835e13c 100644
--- a/ROMFS/px4fmu_common/init.d/3034_fx79
+++ b/ROMFS/px4fmu_common/init.d/3034_fx79
@@ -2,7 +2,7 @@
#
# FX-79 Buffalo Flying Wing
#
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.fw_defaults
diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper
index f3b0e84183301fc88e661880da8dbe7e07e016b4..0f5f5502aa59da4d7b172a56fffec86351fce41d 100644
--- a/ROMFS/px4fmu_common/init.d/3035_viper
+++ b/ROMFS/px4fmu_common/init.d/3035_viper
@@ -2,7 +2,7 @@
#
# Viper
#
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <simon@px4.io>
#
sh /etc/init.d/rc.fw_defaults
diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
index 9a215040351d9cb0caa7f23d92c10dae9b1ccab6..9bfd9d9ed57edd6dee9af6f1c3d58df2978aae8c 100644
--- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
+++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
@@ -1,8 +1,8 @@
#!nsh
#
-# TBS Caipirinha Flying Wing
+# TBS Caipirinha
#
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
#
sh /etc/init.d/rc.fw_defaults
@@ -22,10 +22,6 @@ then
param set FW_PR_I 0
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.03
- param set FW_P_LIM_MAX 45
- param set FW_P_LIM_MIN -45
- param set FW_P_RMAX_NEG 0
- param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 0
param set FW_RR_FF 0.3
param set FW_RR_I 0
diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x
index 06c54a41dba6414240a40c1d5b046289b148b027..8fe8961c5c147b476702a817a7b36691b872538a 100644
--- a/ROMFS/px4fmu_common/init.d/4001_quad_x
+++ b/ROMFS/px4fmu_common/init.d/4001_quad_x
@@ -2,7 +2,7 @@
#
# Generic 10" Quad X geometry
#
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
index e6007db0e71605f27b46a0686a6112af04d65f38..0488e3928e1c8b9df49397a43d441728b4174792 100644
--- a/ROMFS/px4fmu_common/init.d/4008_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -3,9 +3,6 @@
# ARDrone
#
-echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
-
-# Just use the default multicopter settings.
sh /etc/init.d/rc.mc_defaults
#
diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330
index 282ab620daa87d2ec010788b356b7b9add5c2e18..f0cc0520755da1208619c77da6b3eaa5abeae4f0 100644
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -1,8 +1,8 @@
#!nsh
#
-# DJI Flame Wheel F330 Quadcopter
+# DJI Flame Wheel F330
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/4001_quad_x
diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450
index 517b4aa86aeb2278c8aaa4d8bcbdd5b9a3680706..1ca716a6b39ddeab64061066b80495a286af9df4 100644
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -1,8 +1,8 @@
#!nsh
#
-# DJI Flame Wheel F450 Quadcopter
+# DJI Flame Wheel F450
#
-# Lorenz Meier <lm@inf.ethz.ch>
+# Lorenz Meier <lorenz@px4.io>
#
sh /etc/init.d/4001_quad_x
diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can
index 8c5a4fbf2db591f7a51ecc6a1d2d70954ae91202..5c4a6497a0dd8b718ec19d9349418145146dab2c 100644
--- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can
+++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can
@@ -2,7 +2,7 @@
#
# F450-sized quadrotor with CAN
#
-# Lorenz Meier <lm@inf.ethz.ch>
+# Pavel Kirienko <pavel@px4.io>
#
sh /etc/init.d/4001_quad_x
diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
index 99ffd73a5ff9160b33dc44496385a2071f31179f..9fe310ddeec127663e55f6131e99e7a23d6af3f0 100644
--- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
+++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
@@ -3,7 +3,7 @@
# Hobbyking Micro Integrated PCB Quadcopter
# with SimonK ESC firmware and Mystery A1510 motors
#
-# Thomas Gubler <thomasgubler@gmail.com>
+# Thomas Gubler <thomas@px4.io>
#
echo "HK Micro PCB Quad"
diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+
index 1fb25e5d88a68b11c85a8c83ea54149c0543ff74..5512aa738abaee08682c70ee96e8e4bcf8c6a354 100644
--- a/ROMFS/px4fmu_common/init.d/5001_quad_+
+++ b/ROMFS/px4fmu_common/init.d/5001_quad_+
@@ -2,7 +2,7 @@
#
# Generic 10" Quad + geometry
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x
index 34fc6523fef9bc73e150d98e076161c950858622..1043ad8adb5e4ab6c56ba12b581ad174b7418ad1 100644
--- a/ROMFS/px4fmu_common/init.d/6001_hexa_x
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x
@@ -2,12 +2,12 @@
#
# Generic 10" Hexa X geometry
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_x
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+
index 235e376a6eebdd92bb560ed8bef78d11c6ef11c6..84ab88883ce5dd3fecd1ba68ec44cc4370b8bf69 100644
--- a/ROMFS/px4fmu_common/init.d/7001_hexa_+
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+
@@ -2,12 +2,12 @@
#
# Generic 10" Hexa + geometry
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
set MIXER FMU_hexa_+
-# We only can run one channel group with one rate, so set all 8 channels
+# Need to set all 8 channels
set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x
index 769217dc78b7b33d87b14118204bc011b3058dc3..74e304cd96fd0da5e4bed4b2ee2598246dceca7a 100644
--- a/ROMFS/px4fmu_common/init.d/8001_octo_x
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x
@@ -2,7 +2,7 @@
#
# Generic 10" Octo X geometry
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+
index 28b074a545a9c1151cf5696b8a7b762a2512a61f..f7c06c6c80750a1788e5493a21bce95c4ddc935b 100644
--- a/ROMFS/px4fmu_common/init.d/9001_octo_+
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+
@@ -2,7 +2,7 @@
#
# Generic 10" Octo + geometry
#
-# Anton Babushkin <anton.babushkin@me.com>
+# Anton Babushkin <anton@px4.io>
#
sh /etc/init.d/rc.mc_defaults
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 78778d8066ee238c43d3ad3dbd3441f22e6d7169..496a52c5fa9b0560859a7ad91ef8600d8aed9dec 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -1,5 +1,4 @@
#
-# Check if auto-setup from one of the standard scripts is wanted
# SYS_AUTOSTART = 0 means no autostart (default)
#
# AUTOSTART PARTITION:
@@ -18,7 +17,7 @@
# 12000 .. 12999 Octo Cox
#
-# Simulation setups
+# Simulation
#
if param compare SYS_AUTOSTART 901
@@ -53,7 +52,7 @@ then
fi
#
-# Standard plane
+# Plane
#
if param compare SYS_AUTOSTART 2100 100
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index e44cd0953dfce7d18b77dc1f9ebe41012021a414..41e0b149b4cd0bf9c7bc0d56b14f640505cadc64 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -10,7 +10,7 @@ then
#
set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
- #Use the mixer file from the SD-card if it exists
+ # Use the mixer file from the SD-card if it exists
if [ -f $MIXERSD ]
then
set MIXER_FILE $MIXERSD
@@ -54,7 +54,6 @@ then
#
if [ $PWM_RATE != none ]
then
- echo "[init] Set PWM rate: $PWM_RATE"
pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
fi
@@ -63,17 +62,14 @@ then
#
if [ $PWM_DISARMED != none ]
then
- echo "[init] Set PWM disarmed: $PWM_DISARMED"
pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
fi
if [ $PWM_MIN != none ]
then
- echo "[init] Set PWM min: $PWM_MIN"
pwm min -c $PWM_OUTPUTS -p $PWM_MIN
fi
if [ $PWM_MAX != none ]
then
- echo "[init] Set PWM max: $PWM_MAX"
pwm max -c $PWM_OUTPUTS -p $PWM_MAX
fi
fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig
deleted file mode 100644
index e2b5d8f30d94d8161cbb5552b5cae75c9bc658cf..0000000000000000000000000000000000000000
--- a/ROMFS/px4fmu_common/init.d/rc.jig
+++ /dev/null
@@ -1,10 +0,0 @@
-#!nsh
-#
-# Test jig startup script
-#
-
-echo "[testing] doing production test.."
-
-tests jig
-
-echo "[testing] testing done"
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 739df7ac0bfb9a7aa768129172f667e2a4383eec..fbac50cf71ab836a57a835a14757a0de7f9bc171 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -54,7 +54,6 @@ then
fi
fi
-# Start airspeed sensors
if meas_airspeed start
then
echo "[init] Using MEAS airspeed sensor"
@@ -68,16 +67,12 @@ else
fi
fi
-# Check for flow sensor
if px4flow start
then
fi
#
-# Start the sensor collection task.
-# IMPORTANT: this also loads param offsets
-# ALWAYS start this task before the
-# preflight_check.
+# Start sensors -> preflight_check
#
if sensors start
then
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index ea04ece34d805744f91f94affca61c7fbaee8c9e..201b9974910371cb69837c36deed0e43add2a0ed 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -137,9 +137,7 @@ then
#
if param compare SYS_AUTOCONFIG 1
then
- # We can't be sure the defaults haven't changed, so
- # if someone requests a re-configuration, we do it
- # cleanly from scratch (except autostart / autoconfig)
+ # Wipe out params
param reset_nostart
set DO_AUTOCONFIG yes
else
@@ -202,12 +200,10 @@ then
if px4io checkcrc $IO_FILE
then
- echo "[init] PX4IO CRC OK"
echo "PX4IO CRC OK" >> $LOG_FILE
set IO_PRESENT yes
else
- echo "[init] Trying to update"
echo "PX4IO Trying to update" >> $LOG_FILE
tone_alarm MLL32CP8MB
@@ -217,18 +213,15 @@ then
usleep 500000
if px4io checkcrc $IO_FILE
then
- echo "[init] PX4IO CRC OK, update successful"
echo "PX4IO CRC OK after updating" >> $LOG_FILE
tone_alarm MLL8CDE
set IO_PRESENT yes
else
- echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi
else
- echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi
@@ -281,16 +274,12 @@ then
fi
fi
- #
- # Start the datamanager (and do not abort boot if it fails)
- #
+ # waypoint storage
if dataman start
then
fi
- #
- # Start the Commander (needs to be this early for in-air-restarts)
- #
+ # Needs to be this early for in-air-restarts
commander start
#
@@ -424,9 +413,6 @@ then
fi
fi
- #
- # MAVLink
- #
if [ $MAVLINK_FLAGS == default ]
then
# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
@@ -454,15 +440,10 @@ then
# Sensors, Logging, GPS
#
sh /etc/init.d/rc.sensors
-
- #
- # Start logging in all modes, including HIL
- #
sh /etc/init.d/rc.logging
if [ $GPS == yes ]
then
- echo "[init] Start GPS"
if [ $GPS_FAKE == yes ]
then
echo "[init] Faking GPS"
diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh
old mode 100644
new mode 100755
diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py
index b598a65a1a31b6d241fe4bd3ab0403ebf578a7c6..c2da8a20382ddfa0d16d4d421dd705ab5ce3e483 100755
--- a/Tools/px_mkfw.py
+++ b/Tools/px_mkfw.py
@@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string")
parser.add_argument("--summary", action="store", help="set a brief description")
parser.add_argument("--description", action="store", help="set a longer description")
parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity")
+parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file")
parser.add_argument("--image", action="store", help="the firmware image")
args = parser.parse_args()
@@ -101,6 +102,10 @@ if args.git_identity != None:
p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
desc['git_identity'] = str(p.read().strip())
p.close()
+if args.parameter_xml != None:
+ f = open(args.parameter_xml, "rb")
+ bytes = f.read()
+ desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8')
if args.image != None:
f = open(args.image, "rb")
bytes = f.read()
diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh
old mode 100644
new mode 100755
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index d4e461226236a7840e48f14265bc3c7c0a0f9489..3a4540ac06e1853858b62a8b6d11435fe4cbc5e1 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"):
# Load the firmware file
fw = firmware(args.firmware)
-print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
+print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size')))
print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.")
# Spin waiting for a device to show up
diff --git a/Tools/upload.sh b/Tools/upload.sh
new file mode 100755
index 0000000000000000000000000000000000000000..c0dd65eba1670f1e581d75607410f27a637a14ad
--- /dev/null
+++ b/Tools/upload.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+
+EXEDIR=`pwd`
+BASEDIR=$(dirname $0)
+
+SYSTYPE=`uname -s`
+
+#
+# Serial port defaults.
+#
+# XXX The uploader should be smarter than this.
+#
+if [ $SYSTYPE = "Darwin" ];
+then
+SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*"
+fi
+
+if [ $SYSTYPE = "Linux" ];
+then
+SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*"
+fi
+
+if [ $SYSTYPE = "" ];
+then
+SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0"
+fi
+
+python $BASEDIR/px_uploader.py --port $SERIAL_PORTS $1
\ No newline at end of file
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 9fe16fbb6ccea3ad697e2bf7b672106951ef0d24..4507b506c6c0e0a3640d970cd7437bcce636060d 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -132,6 +132,9 @@ MODULES += lib/launchdetection
# Hardware test
#MODULES += examples/hwtest
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
#
# Transitional support - add commands from the NuttX export archive.
#
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index d17dff5bb0700d65e1176822b9bdb017ece9d5ec..d3b8ee93e86997b93e168c74c2f075c14bd310f8 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -40,14 +40,8 @@ MODULES += drivers/meas_airspeed
MODULES += drivers/frsky_telemetry
MODULES += modules/sensors
MODULES += drivers/mkblctrl
-MODULES += drivers/pca8574
MODULES += drivers/px4flow
-
-# Needs to be burned to the ground and re-written; for now,
-# just don't build it.
-#MODULES += drivers/mkblctrl
-
#
# System commands
#
@@ -61,7 +55,6 @@ MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
-MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
@@ -81,10 +74,8 @@ MODULES += modules/uavcan
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
-MODULES += modules/attitude_estimator_so3
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
-MODULES += examples/flow_position_estimator
#
# Vehicle Control
@@ -100,12 +91,6 @@ MODULES += modules/mc_pos_control
#
MODULES += modules/sdlog2
-#
-# Unit tests
-#
-#MODULES += modules/unit_test
-#MODULES += modules/commander/commander_tests
-
#
# Library modules
#
@@ -139,7 +124,7 @@ MODULES += modules/bottle_drop
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
-MODULES += examples/px4_simple_app
+#MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
@@ -156,6 +141,9 @@ MODULES += examples/px4_simple_app
# Hardware test
#MODULES += examples/hwtest
+# Generate parameter XML file
+GEN_PARAM_XML = 1
+
#
# Transitional support - add commands from the NuttX export archive.
#
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index 67934b8e4680cad3fe2423dae2ab0faa01844113..8aa8badc3c3795cc7fa15dfd164e6aae55dd04af 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -53,7 +53,14 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += platforms/nuttx
#
-# Tests
+# Modules to test-build, but not useful for test environment
+#
+MODULES += modules/attitude_estimator_so3
+MODULES += drivers/pca8574
+MODULES += examples/flow_position_estimator
+
+#
+# Libraries
#
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk
index 60602e76fe57c57b3ca24ce9c16743f5872b017b..21e8739aa1ed2d41f7af3aa0f9af1d54ed94a504 100644
--- a/makefiles/firmware.mk
+++ b/makefiles/firmware.mk
@@ -467,6 +467,7 @@ endif
PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4
PRODUCT_BIN = $(WORK_DIR)firmware.bin
PRODUCT_ELF = $(WORK_DIR)firmware.elf
+PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml
.PHONY: firmware
firmware: $(PRODUCT_BUNDLE)
@@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
$(PRODUCT_BUNDLE): $(PRODUCT_BIN)
@$(ECHO) %% Generating $@
+ifdef GEN_PARAM_XML
+ python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml
$(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
--git_identity $(PX4_BASE) \
+ --parameter_xml $(PRODUCT_PARAMXML) \
--image $< > $@
+else
+ $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \
+ --git_identity $(PX4_BASE) \
+ --image $< > $@
+endif
$(PRODUCT_BIN): $(PRODUCT_ELF)
$(call SYM_TO_BIN,$<,$@)
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 71c7fb49fad36330bf752b2c01924786dab6820b..84e5cd08a221f1ae847cd6f7d0a1ff053ba7b923 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump
# Check if the right version of the toolchain is available
#
-CROSSDEV_VER_SUPPORTED = 4.7
+CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)
-ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND)))
-$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x)
+ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
+$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED))
endif
diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index 90383fac84d031aef17989a1497c2473dfa64340..ad5e5a645dec152419264ad32221f7c113ea5c30 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 90383fac84d031aef17989a1497c2473dfa64340
+Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30
diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs
index 7b2ea703a262e06781567d6e8588b0b7617194fa..e7318e51963196db8d6780090dff213080b00c8b 100644
--- a/nuttx-configs/px4fmu-v1/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
-MAXOPTIMIZATION = -O3
+MAXOPTIMIZATION = -Os
ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index e70320aaa4c9d1e1fca8980712f4b49acfa479aa..f3ce53b4aed73ca754158fe77f535cf0ec4e01e1 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
-MAXOPTIMIZATION = -O3
+MAXOPTIMIZATION = -Os
ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script
index 1be39fb87f0ab04f90fc76d4100ecdcef328a962..bec896d1ce53b1cf2827d4f46d1bb9c0a1dcc0f7 100644
--- a/nuttx-configs/px4fmu-v2/scripts/ld.script
+++ b/nuttx-configs/px4fmu-v2/scripts/ld.script
@@ -50,7 +50,8 @@
MEMORY
{
- flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K
+ /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */
+ flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
}
diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs
index 712631f47142486e71b244d0367c9fd82bd23e48..7a0792ff64c9db6f929488185bc8360a5d03b6d6 100644
--- a/nuttx-configs/px4io-v1/nsh/Make.defs
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
-MAXOPTIMIZATION = -O3
+MAXOPTIMIZATION = -Os
ARCHCPUFLAGS = -mcpu=cortex-m3 \
-mthumb \
-march=armv7-m
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
index cd2d8eba3be80b122ff6322434bee77214f98259..1717464d2f5a2dbea3aeee8c95c9535db0854420 100644
--- a/nuttx-configs/px4io-v2/nsh/Make.defs
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -53,15 +53,11 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
-MAXOPTIMIZATION = -O3
+MAXOPTIMIZATION = -Os
ARCHCPUFLAGS = -mcpu=cortex-m3 \
-mthumb \
-march=armv7-m
-# enable precise stack overflow tracking
-#INSTRUMENTATIONDEFINES = -finstrument-functions \
-# -ffixed-r10
-
# use our linker script
LDSCRIPT = ld.script
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 293690d2744cf9328b63681bff1db267c020e226..3a1e1b7b51a95c7fb93032dad1bd7d1bd6e4f904 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -147,7 +147,7 @@ Airspeed::init()
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0)
- warnx("failed to create airspeed sensor object. uORB started?");
+ warnx("uORB started?");
}
ret = OK;
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index e5bb772b3c53ea3396ff9d325a00fd973c3b1b0e..9d2c1441dea9ba81b621816375e468f46b5d4556 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -89,8 +89,8 @@ static void
usage(const char *reason)
{
if (reason)
- fprintf(stderr, "%s\n", reason);
- fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+ warnx("%s\n", reason);
+ warnx("usage: {start|stop|status} [-d <UART>]\n\n");
exit(1);
}
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
- printf("ardrone_interface already running\n");
+ warnx("already running\n");
/* this is not an error */
exit(0);
}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
- printf("\tardrone_interface is running\n");
+ warnx("running");
} else {
- printf("\tardrone_interface not started\n");
+ warnx("not started");
}
exit(0);
}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
- fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+ warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
- fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+ warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
- fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+ warnx("ERR: tcsetattr: %s", uart_name);
close(uart);
return -1;
}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
char *device = "/dev/ttyS1";
- /* welcome user */
- printf("[ardrone_interface] Control started, taking over motors\n");
-
/* File descriptors */
int gpios;
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
struct termios uart_config_original;
if (motor_test_mode) {
- printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+ warnx("setting 10 %% thrust.\n");
}
/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
- printf("[ardrone_interface] Motors initialized - ready.\n");
- fflush(stdout);
-
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
- fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+ warnx("No UART, exiting.");
thread_running = false;
exit(ERROR);
}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
- fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+ warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
- fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+ warnx("write fail");
thread_running = false;
exit(ERROR);
}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
- fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+ warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
- fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+ warnx("ERR: tcsetattr");
}
- printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index fc017dd589eb4f5c737480ed6268f91846e63a4b..4fa24275f1e3740c913f4b72e547409982a2a7c7 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
- fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+ warnx("Failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
index b53fe0a2939d2c4688bd2e7fa0d22960d119a15d..0a2d910091f0de61a1f756bff42affccabd030b1 100644
--- a/src/drivers/boards/aerocore/module.mk
+++ b/src/drivers/boards/aerocore/module.mk
@@ -6,3 +6,5 @@ SRCS = aerocore_init.c \
aerocore_pwm_servo.c \
aerocore_spi.c \
aerocore_led.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917332c4fd78f6c9d68b188f363a2fee44..5e1a27d5a12e8a0a3a7c350d0307b65c25da7333 100644
--- a/src/drivers/boards/px4fmu-v1/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu_led.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
index 99d37eecac51f90da035eacd049264b19434c08a..103232b0ca82efd58a90ba5e0a0a57c3da749f75 100644
--- a/src/drivers/boards/px4fmu-v2/module.mk
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu2_led.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15ae2ba75ae66547f4d2e4a51db591770..a7a14dd0723d8b388e87a4a672fdf390e8cd5294 100644
--- a/src/drivers/boards/px4io-v1/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
@@ -4,3 +4,5 @@
SRCS = px4io_init.c \
px4io_pwm_servo.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
index 85f94e8be1c06e339fca407bd02dc88065078e6f..3f0e9a0b3a22d42728aba6f14bbd1d922b16cba7 100644
--- a/src/drivers/boards/px4io-v2/module.mk
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -4,3 +4,5 @@
SRCS = px4iov2_init.c \
px4iov2_pwm_servo.c
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 9d684e394847c5fdbd8b11efc9d65c56b33cb766..67aaa0affcbb745bd54fc9c4bc5fe8f80fd56e70 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -130,7 +130,8 @@ public:
enum DeviceBusType {
DeviceBusType_UNKNOWN = 0,
DeviceBusType_I2C = 1,
- DeviceBusType_SPI = 2
+ DeviceBusType_SPI = 2,
+ DeviceBusType_UAVCAN = 3,
};
/*
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 6873f24b601df8a05ed1c1d5c165ee42bc8b564e..b10c3e18a99b735504c817038b0a1f887f6db81d 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -117,6 +117,23 @@ struct pwm_output_values {
unsigned channel_count;
};
+
+/**
+ * RC config values for a channel
+ *
+ * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
+ * param_get() dependency
+ */
+struct pwm_output_rc_config {
+ uint8_t channel;
+ uint16_t rc_min;
+ uint16_t rc_trim;
+ uint16_t rc_max;
+ uint16_t rc_dz;
+ uint16_t rc_assignment;
+ bool rc_reverse;
+};
+
/*
* ORB tag for PWM outputs.
*/
@@ -214,7 +231,19 @@ ORB_DECLARE(output_pwm);
#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25)
/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26)
+#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26)
+
+/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
+#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27)
+
+/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28)
+
+/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29)
+
+/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
+#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30)
/*
*
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 6e08390436cad1abb5923a36a0177b1113bb1068..bccdf11906ad2de597c376572080f199308a07f5 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
- warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+ warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
static const speed_t speed = B9600;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
- warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+ warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
- warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+ warnx("ERR: %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
}
- /* Print welcome text */
- warnx("FrSky telemetry interface starting...");
-
/* Open UART */
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5fb4b9ff8da4f2027f57576e2d1d4c31d7f0d248..47c907bd33130a6b6387b779a405422c80266abb 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
void
GPS::task_main()
{
- log("starting");
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR);
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index f0dc0c6516b9f9a556ca738d618c4a9fda36a4ee..9b5c8133b4d69122764173a1610329a66ef11662 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -442,8 +442,6 @@ HIL::task_main()
/* make sure servos are off */
// up_pwm_servo_deinit();
- log("stopping");
-
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index e4ecfa6b5d37739dd7e512d6ed03c9ba6b336f86..81f767965027e792b2118160b7bcaf52339c3d54 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
- warnx("starting mag scale calibration");
-
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
- warn("failed to set 2Hz poll rate");
+ warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
- warnx("failed to set 2.5 Ga range");
+ warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
- warnx("failed to enable sensor calibration mode");
+ warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
- warn("WARNING: failed to get scale / offsets for mag");
+ warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
- warn("WARNING: failed to set null scale / offsets for mag");
+ warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
- warn("timed out waiting for sensor data");
+ warn("ERROR: TIMEOUT 1");
goto out;
}
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
- warn("periodic read failed");
+ warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
- warn("timed out waiting for sensor data");
+ warn("ERROR: TIMEOUT 2");
goto out;
}
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
- warn("periodic read failed");
+ warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
-
- //warnx("periodic read %u", i);
- //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
- //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
- warn("failed calibration");
ret = -EIO;
goto out;
}
-#if 0
- warnx("measurement avg: %.6f %.6f %.6f",
- (double)sum_excited[0]/good_count,
- (double)sum_excited[1]/good_count,
- (double)sum_excited[2]/good_count);
-#endif
-
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
- warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
- warn("failed to set new scale / offsets for mag");
+ warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
- warnx("failed to set 1.1 Ga range");
+ warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
- warnx("failed to disable sensor calibration mode");
+ warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
- if (!check_scale()) {
- warnx("mag scale calibration successfully finished.");
- } else {
- warnx("mag scale calibration finished with invalid results.");
+ if (check_scale()) {
+ /* failed */
+ warnx("FAILED: SCALE");
ret = ERROR;
}
- } else {
- warnx("mag scale calibration failed.");
}
return ret;
diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
index 5daa01dc57dcb901dd12b7f56a111f7302ada4b2..be2ee72766224ea7d7655cbede9a48a3a198461e 100644
--- a/src/drivers/hmc5883/module.mk
+++ b/src/drivers/hmc5883/module.mk
@@ -42,3 +42,5 @@ SRCS = hmc5883.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
index cb8bbba37e6a009179e670d08009f04a5fa67b70..60a49b559c235034a6536775dbe05b4e04f0ce55 100644
--- a/src/drivers/hott/comms.cpp
+++ b/src/drivers/hott/comms.cpp
@@ -55,7 +55,7 @@ open_uart(const char *device)
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
- err(1, "Error opening port: %s", device);
+ err(1, "ERR: opening %s", device);
}
/* Back up the original uart configuration to restore it after exit */
@@ -63,7 +63,7 @@ open_uart(const char *device)
struct termios uart_config_original;
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
- err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+ err(1, "ERR: %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
- err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+ err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
close(uart);
- err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+ err(1, "ERR: %s (tcsetattr)", device);
}
/* Activate single wire mode */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index a3d3a39333f06c79681ff8fae76715a63df730c8..8ab9d8d55f51ddcf43ebeb577e18dfeeff2be193 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
- warnx("deamon already running");
+ warnx("already running");
exit(0);
}
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
- warnx("daemon is running");
+ warnx("is running");
} else {
- warnx("daemon not started");
+ warnx("not started");
}
exit(0);
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index d293f9954e3431691bd432d560e8427cab5483f4..edbb14172eb25fe08de315d09dd4e559a9bb0529 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
- warnx("deamon already running");
+ warnx("already running");
exit(0);
}
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
- warnx("daemon is running");
+ warnx("is running");
} else {
- warnx("daemon not started");
+ warnx("not started");
}
exit(0);
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
index 5630e7aeca228e31574438fbac9b6afda3b34f74..3d64d62be0d9efa5231a93c7b709c54317601d17 100644
--- a/src/drivers/l3gd20/module.mk
+++ b/src/drivers/l3gd20/module.mk
@@ -8,3 +8,5 @@ SRCS = l3gd20.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index b4f3974f4da576041d7650d809d3ff2a09282fcb..0421eb113ec1400ff3d35b8f681eb183fbec888f 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -8,3 +8,5 @@ SRCS = lsm303d.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1d9a463ad3dbc3f34e75b6a7b9eca2b21b3f1db4..ba46de379810cfea5694845bc2b88ce702f80937 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -519,7 +519,7 @@ test()
ret = poll(&fds, 1, 2000);
if (ret != 1) {
- errx(1, "timed out waiting for sensor data");
+ errx(1, "timed out");
}
/* now go get it */
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
index 5b4893b127830ff2d9d8dfb03f7af87008689746..da9fcc0fc429bdc78dbeb76f2aa687e83794deb9 100644
--- a/src/drivers/mpu6000/module.mk
+++ b/src/drivers/mpu6000/module.mk
@@ -42,3 +42,5 @@ SRCS = mpu6000.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 6c68636c6d3fcdf44170d54191c244cb3ace885b..804027b056fe8d619f6c0fd1291837e83abb0b7a 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -76,7 +76,7 @@
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
-
+
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
- _debug_enabled = true;
+ _debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@@ -240,7 +240,7 @@ PX4FLOW::init()
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0) {
- debug("failed to create px4flow object. Did you start uOrb?");
+ warnx("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
@@ -442,8 +442,6 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
- log("i2c::transfer returned %d", ret);
- printf("i2c::transfer flow returned %d");
return ret;
}
@@ -465,7 +463,7 @@ PX4FLOW::collect()
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0) {
- log("error reading from sensor: %d", ret);
+ debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@@ -565,7 +563,7 @@ PX4FLOW::cycle()
/* perform collection */
if (OK != collect()) {
- log("collection error");
+ debug("collection error");
/* restart the measurement state machine */
start();
return;
@@ -592,7 +590,7 @@ PX4FLOW::cycle()
/* measurement phase */
if (OK != measure()) {
- log("measure error");
+ debug("measure error");
}
/* next phase is collection */
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index a60f1a434af72d219054fbb9cca7ab33cb22cec9..a06323a52f20f0d07e2462d6298c87f455c1f538 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -8,3 +8,5 @@ SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 5b838fb75e189592629dad5972893ebd98ee0473..924283356bf37d40d63db62073284b5855381aaa 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fd9eb41709538913b5c1382765e0700c26483b8b..519ba663a1e1f97133bb02f56bf6896843693a3a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1245,34 +1245,40 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
- param_get(param_find("RC_MAP_ROLL"), &ichan);
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ /* ROLL */
+ param_get(param_find("RC_MAP_ROLL"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 0;
+ }
+ /* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 1;
+ }
+ /* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 2;
+ }
+ /* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 3;
+ }
+ /* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 4;
+ }
+ /* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
@@ -1651,10 +1657,6 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
- /* if no FMU comms(!) just don't publish */
- if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
- return OK;
-
/* data we are going to fetch */
actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
@@ -2055,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
- printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
+ printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2065,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
+ ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2305,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
+ case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
+ /* control whether override on FMU failure is
+ immediate or waits for override threshold on mode
+ switch */
+ if (arg == 0) {
+ /* clear override immediate flag */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
+ } else {
+ /* set override immediate flag */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
+ }
+ break;
+
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2566,6 +2582,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
+ case PWM_SERVO_SET_RC_CONFIG: {
+ /* enable setting of RC configuration without relying
+ on param_get()
+ */
+ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
+ if (config->channel >= _max_actuators) {
+ /* fail with error */
+ return E2BIG;
+ }
+
+ /* copy values to registers in IO */
+ uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+ uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
+ regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min;
+ regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim;
+ regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max;
+ regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz;
+ regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
+ regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+ if (config->rc_reverse) {
+ regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+ }
+ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+ break;
+ }
+
+ case PWM_SERVO_SET_OVERRIDE_OK:
+ /* set the 'OVERRIDE OK' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
+ break;
+
+ case PWM_SERVO_CLEAR_OVERRIDE_OK:
+ /* clear the 'OVERRIDE OK' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+ break;
+
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
index 48620feea81993e609c35656c9a9ece6c2267b86..38ea490a0e8502eb62dc4706db073b35d4a65969 100644
--- a/src/drivers/stm32/adc/module.mk
+++ b/src/drivers/stm32/adc/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND = adc
SRCS = adc.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
index bb751c7f689c027a7080988a5ec1d71ae72fd866..54428e270d2b365a431c937c847dfb33fb31cdc4 100644
--- a/src/drivers/stm32/module.mk
+++ b/src/drivers/stm32/module.mk
@@ -41,3 +41,5 @@ SRCS = drv_hrt.c \
drv_pwm_servo.c
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION = -Os
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
index 827cf30b27b33b97158377e3f6da62dfd83f5059..25a194ef61f37c695b1cf585024cbb70c28a93db 100644
--- a/src/drivers/stm32/tone_alarm/module.mk
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm
SRCS = tone_alarm.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION = -Os
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4cbbad7de178787e29193af5dc7cc5f9..cfcc48b62a0116eabdcb47a3dcf1e0c965383de0 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
- // Calculate PD + FF throttle
+ // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
_throttle_dem = constrain(_throttle_dem,
- _last_throttle_dem - thrRateIncr,
- _last_throttle_dem + thrRateIncr);
+ _last_throttle_dem - thrRateIncr,
+ _last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
- // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
-
// Calculate integrator state upper and lower limits
- // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+ // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
- // Set integrator to a max throttle value dduring climbout
+ // Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 35dc39ec6a8826d6ccafc85fcccbc257857697d8..e2dbc30dd5cc5ec880d73694439413c7e21a9c24 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
- // print text
- printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
- fflush(stdout);
-
int overloadcounter = 19;
/* Initialize filter */
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 31c9157e110ac0f90ae3673ca6a105bd2ae04c92..e0bcbc6e9ee1585c53f43575aba2c9692e8b35e8 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -223,7 +223,7 @@ BottleDrop::start()
_main_task = task_spawn_cmd("bottle_drop",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
- 2048,
+ 1500,
(main_t)&BottleDrop::task_main_trampoline,
nullptr);
@@ -283,7 +283,6 @@ BottleDrop::drop()
// force the door open if we have to
if (_doors_opened == 0) {
open_bay();
- warnx("bay not ready, forced open");
}
while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
- mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+ mavlink_log_critical(_mavlink_fd, "drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
- mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+ mavlink_log_critical(_mavlink_fd, "opening bay");
} else {
lock_release();
close_bay();
- mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+ mavlink_log_critical(_mavlink_fd, "closing bay");
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
- mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+ mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
break;
case 1:
_drop_approval = true;
- mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+ mavlink_log_critical(_mavlink_fd, "got drop position and approval");
break;
default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
break;
case VEHICLE_CMD_RESULT_DENIED:
- mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+ mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_FAILED:
- mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+ mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
- mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+ mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
- mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+ mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
break;
default:
diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
index 6b18fff556bfdfa7154ee3f6620b58463e0aa26f..df9f5473b0426f1c4db6ba6f91b459bc8bfa8ea3 100644
--- a/src/modules/bottle_drop/module.mk
+++ b/src/modules/bottle_drop/module.mk
@@ -39,3 +39,7 @@ MODULE_COMMAND = bottle_drop
SRCS = bottle_drop.cpp \
bottle_drop_params.c
+
+MAXOPTIMIZATION = -Os
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd4698502a51397dd9865f71ce68873e819..e081955d0707a875a676194a1d61694df26876e4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "arm")) {
- arm_disarm(true, mavlink_fd, "command line");
+ int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+ arm_disarm(true, mavlink_fd_local, "command line");
+ close(mavlink_fd_local);
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
- arm_disarm(false, mavlink_fd, "command line");
+ int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+ arm_disarm(false, mavlink_fd_local, "command line");
+ close(mavlink_fd_local);
exit(0);
}
@@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[])
nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
+ nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
+ nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
+ nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
@@ -1848,7 +1853,11 @@ int commander_thread_main(int argc, char *argv[])
if (status.failsafe != failsafe_old) {
status_changed = true;
- mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+ if (status.failsafe) {
+ mavlink_log_critical(mavlink_fd, "failsafe mode on");
+ } else {
+ mavlink_log_critical(mavlink_fd, "failsafe mode off");
+ }
failsafe_old = status.failsafe;
}
@@ -2196,18 +2205,6 @@ set_control_mode()
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;
- control_mode.flag_control_rates_enabled = true;
- control_mode.flag_control_attitude_enabled = false;
- control_mode.flag_control_altitude_enabled = false;
- control_mode.flag_control_climb_rate_enabled = false;
- 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_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
@@ -2220,64 +2217,6 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
- case NAVIGATION_STATE_OFFBOARD:
- control_mode.flag_control_manual_enabled = false;
- control_mode.flag_control_auto_enabled = false;
- control_mode.flag_control_offboard_enabled = true;
-
- switch (sp_offboard.mode) {
- case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
- control_mode.flag_control_rates_enabled = true;
- control_mode.flag_control_attitude_enabled = false;
- control_mode.flag_control_altitude_enabled = false;
- control_mode.flag_control_climb_rate_enabled = false;
- control_mode.flag_control_position_enabled = false;
- control_mode.flag_control_velocity_enabled = false;
- break;
-
- case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
- 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 = false;
- control_mode.flag_control_position_enabled = false;
- control_mode.flag_control_velocity_enabled = false;
- break;
-
- case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
- control_mode.flag_control_rates_enabled = true;
- control_mode.flag_control_attitude_enabled = false;
- control_mode.flag_control_force_enabled = true;
- control_mode.flag_control_altitude_enabled = false;
- control_mode.flag_control_climb_rate_enabled = false;
- control_mode.flag_control_position_enabled = false;
- control_mode.flag_control_velocity_enabled = false;
- break;
-
- case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
- case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
- case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
- case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
- 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;
- //XXX: the flags could depend on sp_offboard.ignore
- break;
-
- default:
- control_mode.flag_control_rates_enabled = false;
- control_mode.flag_control_attitude_enabled = false;
- control_mode.flag_control_altitude_enabled = false;
- control_mode.flag_control_climb_rate_enabled = false;
- 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;
@@ -2293,6 +2232,7 @@ set_control_mode()
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;
@@ -2318,6 +2258,19 @@ set_control_mode()
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;
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = false;
+ control_mode.flag_control_altitude_enabled = false;
+ control_mode.flag_control_climb_rate_enabled = false;
+ 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;
@@ -2331,6 +2284,19 @@ set_control_mode()
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;
@@ -2344,6 +2310,63 @@ set_control_mode()
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;
+ control_mode.flag_control_offboard_enabled = true;
+
+ switch (sp_offboard.mode) {
+ case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = false;
+ control_mode.flag_control_altitude_enabled = false;
+ control_mode.flag_control_climb_rate_enabled = false;
+ control_mode.flag_control_position_enabled = false;
+ control_mode.flag_control_velocity_enabled = false;
+ break;
+
+ case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+ 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 = false;
+ control_mode.flag_control_position_enabled = false;
+ control_mode.flag_control_velocity_enabled = false;
+ break;
+
+ case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = false;
+ control_mode.flag_control_force_enabled = true;
+ control_mode.flag_control_altitude_enabled = false;
+ control_mode.flag_control_climb_rate_enabled = false;
+ control_mode.flag_control_position_enabled = false;
+ control_mode.flag_control_velocity_enabled = false;
+ break;
+
+ case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+ case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+ case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+ case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
+ 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;
+ //XXX: the flags could depend on sp_offboard.ignore
+ break;
+
+ default:
+ control_mode.flag_control_rates_enabled = false;
+ control_mode.flag_control_attitude_enabled = false;
+ control_mode.flag_control_altitude_enabled = false;
+ control_mode.flag_control_climb_rate_enabled = false;
+ control_mode.flag_control_position_enabled = false;
+ control_mode.flag_control_velocity_enabled = false;
+ }
+ break;
+
default:
break;
}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9568752aedcd870836d80bb272b9e5c7822416a0..e37019d02d4b66c9157e626dc244b21b11f5f9fa 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
- const bool stay_in_failsafe)
+ const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_MISSION:
+
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
- * - if either the datalink is enabled and lost as well as RC is lost
- * - if there is no datalink and the mission is finished */
+ * - depending on datalink, RC and if the mission is finished */
+
+ /* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
- /* Finished handling commands which have priority , now handle failures */
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+ /* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
- } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
- (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+ /* datalink loss enabled:
+ * check for datalink lost: this should always trigger RTGS */
+ } else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
- /* also go into failsafe if just datalink is lost */
- } else if (status->data_link_lost && data_link_loss_enabled) {
+ /* datalink loss disabled:
+ * check if both, RC and datalink are lost during the mission
+ * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+ } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+ (status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
- /* don't bother if RC is lost and mission is not yet finished */
- } else if (status->rc_signal_lost && !stay_in_failsafe) {
-
- /* this mode is ok, we don't need RC for missions */
- status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+ /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
- /* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index f0139a4b70d50d14af38c29427f8a8b3daf17413..2860d1efceacfd2c0f025fd1be06e601408cc59c 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -39,3 +39,5 @@ SRCS = test_params.c \
block/BlockParam.cpp \
uorb/blocks.cpp \
blocks.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index b2355d4d895b2560b4ed99c7941d623c139bde7b..705e54be32a09c4604786b9cc13906cd73b383c9 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
{
work_q_item_t *work;
- /* inform about start */
- warnx("Initializing..");
-
/* Initialize global variables */
g_key_offsets[0] = 0;
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
- }
- else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+ } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
- }
- else
+ } else {
warnx("Unknown restart");
- }
- else
+ }
+ } else {
warnx("Unknown restart");
+ }
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 685f5e12f1f2eb4f2d6726990e85ade00b397b0e..e7805daa915b3f45f87091468b28490d1e8bb8c5 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
warn_index = max_warn_index;
}
- warnx("reset: %s", feedback[warn_index]);
- mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+ // Do not warn about accel offset if we have no position updates
+ if (!(warn_index == 5 && _ekf->staticMode)) {
+ warnx("reset: %s", feedback[warn_index]);
+ mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+ }
}
struct estimator_status_report rep;
@@ -1557,7 +1560,7 @@ FixedwingEstimator::start()
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
- 5000,
+ 7500,
(main_t)&FixedwingEstimator::task_main_trampoline,
nullptr);
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index bec706badd26af7147d72e12dc179142b3c52ded..9afe74af1cb11b7cfc9d3ad50c077b65562d8a58 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
- 50,
- MAV_TYPE_FIXED_WING,
- 0,
- 0,
- 0
+ 50
}; // System ID, 1-255, Component/Subsystem ID, 1-255
/*
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6b4edff78a11a20efdfc6d07c0e053492317577e..fb9f65cf50312d36d155ddca4eb5ea6fa48b0bd0 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
- _param_system_type(0),
+ _param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
+ _param_forward_externalsp(0),
+ _system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
param_get(_param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
- mavlink_system.type = system_type;
+ _system_type = system_type;
}
int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
pthread_mutex_lock(&_send_mutex);
- int buf_free = get_free_tx_buf();
+ unsigned buf_free = get_free_tx_buf();
uint8_t payload_len = mavlink_message_lengths[msgid];
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
pthread_mutex_lock(&_send_mutex);
- int buf_free = get_free_tx_buf();
+ unsigned buf_free = get_free_tx_buf();
_last_write_try_time = hrt_absolute_time();
unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
/* check if there is space in the buffer, let it overflow else */
- if (buf_free < TX_BUFFER_GAP) {
+ if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
/* no enough space in buffer to send */
count_txerr();
count_txerrbytes(packet_len);
@@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
- 2900,
+ 2800,
(main_t)&Mavlink::start_helper,
(const char **)argv);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f96e586bbe16f9b0056e7e82c3b84b5db17eacf..ad5e5001b9e226189f4250593f4818302b13afda 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -265,6 +265,8 @@ public:
struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; }
+ unsigned get_system_type() { return _system_type; }
+
protected:
Mavlink *next;
@@ -354,6 +356,8 @@ private:
param_t _param_use_hil_gps;
param_t _param_forward_externalsp;
+ unsigned _system_type;
+
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf9e865f108f724f874c2e78cae8b69d0..978aee1185f6a78b3f4fb04508535b258850a806 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -302,7 +302,7 @@ protected:
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
- msg.type = mavlink_system.type;
+ msg.type = _mavlink->get_system_type();
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
@@ -382,11 +382,11 @@ protected:
clock_gettime(CLOCK_REALTIME, &ts);
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
- struct tm t;
- gmtime_r(&gps_time_sec, &t);
+ struct tm tt;
+ gmtime_r(&gps_time_sec, &tt);
// XXX we do not want to interfere here with the SD log app
- strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+ strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
fp = fopen(log_file_path, "ab");
}
@@ -1353,15 +1353,17 @@ protected:
const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
+ unsigned system_type = _mavlink->get_system_type();
+
/* scale outputs depending on system type */
- if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
- mavlink_system.type == MAV_TYPE_HEXAROTOR ||
- mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+ if (system_type == MAV_TYPE_QUADROTOR ||
+ system_type == MAV_TYPE_HEXAROTOR ||
+ system_type == MAV_TYPE_OCTOROTOR) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
- switch (mavlink_system.type) {
+ switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
@@ -1717,7 +1719,53 @@ protected:
msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
- msg.rssi = rc.rssi;
+
+ /* RSSI has a max value of 100, and when Spektrum or S.BUS are
+ * available, the RSSI field is invalid, as they do not provide
+ * an RSSI measurement. Use an out of band magic value to signal
+ * these digital ports. XXX revise MAVLink spec to address this.
+ * One option would be to use the top bit to toggle between RSSI
+ * and input source mode.
+ *
+ * Full RSSI field: 0b 1 111 1111
+ *
+ * ^ If bit is set, RSSI encodes type + RSSI
+ *
+ * ^ These three bits encode a total of 8
+ * digital RC input types.
+ * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+ * ^ These four bits encode a total of
+ * 16 RSSI levels. 15 = full, 0 = no signal
+ *
+ */
+
+ /* Initialize RSSI with the special mode level flag */
+ msg.rssi = (1 << 7);
+
+ /* Set RSSI */
+ msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+ switch (rc.input_source) {
+ case RC_INPUT_SOURCE_PX4FMU_PPM:
+ /* fallthrough */
+ case RC_INPUT_SOURCE_PX4IO_PPM:
+ msg.rssi |= (0 << 4);
+ break;
+ case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+ msg.rssi |= (1 << 4);
+ break;
+ case RC_INPUT_SOURCE_PX4IO_SBUS:
+ msg.rssi |= (2 << 4);
+ break;
+ case RC_INPUT_SOURCE_PX4IO_ST24:
+ msg.rssi |= (3 << 4);
+ break;
+ }
+
+ if (rc.rc_lost) {
+ /* RSSI is by definition zero */
+ msg.rssi = 0;
+ }
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bc092c7e9c034a2061cf2cff9d67fc355ea995fc..ca00d1a67dcc14ff5b33f6853206024a1ad000c9 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
offboard_control_sp.ignore &= ~(1 << i);
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
}
+
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
- offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
- OFB_IGN_BIT_YAW;
+ if (set_position_target_local_ned.type_mask & (1 << 10)) {
+ offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW);
+ }
+
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
- offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
- OFB_IGN_BIT_YAWRATE;
+ if (set_position_target_local_ned.type_mask & (1 << 11)) {
+ offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE);
+ }
offboard_control_sp.timestamp = hrt_absolute_time();
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 19c10198c2d34039682c9250973517159d0d8558..81a13edb8f62ec02c325d32d0e3cb042d0c793af 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
void
MulticopterAttitudeControl::task_main()
{
- warnx("started");
- fflush(stdout);
/*
* do subscriptions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d52138522c8531f78ad84b277007457c22ef113d..5918d6bc5fe2306017df1df8f0f4c55db2ae1236 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
- mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+ mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
}
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
- mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+ mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
@@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt)
void
MulticopterPositionControl::task_main()
{
- warnx("started");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
- mavlink_log_info(_mavlink_fd, "[mpc] started");
/*
* do subscriptions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c731972986eeebaa7874013d0341600d66..e789fd10de64b60c43dc9bcb72ba96408082606d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a611eb35872f9df90e248bf9d9970eb850..0765e9b7c06ee730d2d9488ed57a65dee1648966 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
- mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
- if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
- /* mission item index out of bounds */
- return false;
- }
-
- /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+ /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+ * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
+
+ if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+ /* mission item index out of bounds */
+ return false;
+ }
+
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (is_current) {
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
- if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+ &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "ERROR DO JUMP waypoint could not be written");
+ "ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
- mavlink_log_critical(_navigator->get_mavlink_fd(),
- "DO JUMP repetitions completed");
+ if (is_current) {
+ mavlink_log_critical(_navigator->get_mavlink_fd(),
+ "DO JUMP repetitions completed");
+ }
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
void
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4cb5aebf62d82b31771b595fda80158d8..9cd609955b4f1b92415ed661c25a334b76d14910 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
+ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0daf58182bacd9320034d208cc97e6fc8f..df620e5e7ff2a79b738323ba1bb0cb384689a597 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
_gpsFailure(this, "GPSF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
+ _pos_sp_triplet_published_invalid_once(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -289,8 +290,9 @@ Navigator::task_main()
navigation_capabilities_update();
params_update();
- /* rate limit position updates to 50 Hz */
+ /* rate limit position and sensor updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
+ orb_set_interval(_sensor_combined_sub, 20);
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
@@ -426,12 +428,15 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
+ _pos_sp_triplet_published_invalid_once = false;
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
} else {
@@ -439,11 +444,13 @@ Navigator::task_main()
}
break;
case NAVIGATION_STATE_AUTO_RTL:
- _navigation_mode = &_rtl;
+ _pos_sp_triplet_published_invalid_once = false;
+ _navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
+ _pos_sp_triplet_published_invalid_once = false;
if (_param_datalinkloss_obc.get() != 0) {
_navigation_mode = &_dataLinkLoss;
} else {
@@ -451,9 +458,11 @@ Navigator::task_main()
}
break;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
default:
@@ -467,9 +476,9 @@ Navigator::task_main()
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
- /* if nothing is running, set position setpoint triplet invalid */
- if (_navigation_mode == nullptr) {
- // TODO publish empty sp only once
+ /* if nothing is running, set position setpoint triplet invalid once */
+ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+ _pos_sp_triplet_published_invalid_once = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
@@ -497,8 +506,8 @@ Navigator::start()
/* start the task */
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 5,
- 2000,
+ SCHED_PRIORITY_DEFAULT + 20,
+ 1800,
(main_t)&Navigator::task_main_trampoline,
nullptr);
@@ -630,9 +639,6 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
- /* reset reached bool */
- _mission_result.reached = false;
- _mission_result.finished = false;
}
void
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42ecce8476296243f1416525c67fa91a9..42392e73991b3e9ecad2885de50f24fb74f08051 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
break;
case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81bbaa658732bf502f01ef8115ff29105c2aa817..e736a86d77b68105e3347a2b6c3bf9e839877fe2 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = true;
} else {
- warnx("app not started");
+ warnx("not started");
}
exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
- warnx("app is running");
+ warnx("is running");
} else {
- warnx("app not started");
+ warnx("not started");
}
exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
- warnx("started");
int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
- mavlink_log_info(mavlink_fd, "[inav] started");
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
@@ -389,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
- warnx("baro offs: %.2f", (double)baro_offset);
- mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
+ warnx("baro offs: %d", (int)baro_offset);
+ mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@@ -520,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
- mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+ mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
}
} else {
@@ -721,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* initialize projection */
map_projection_init(&ref, lat, lon);
- warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
- mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
+ // XXX replace this print
+ warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
+ mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e0dc94960b920e2b903da31b773a5e19..58c9429b6ff9065dd2eb9596eb6909cb32b3c2f9 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
#include <stdbool.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
#include <rc/st24.h>
@@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
if (*dsm_updated) {
- r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
for (unsigned i = 0; i < n_bytes; i++) {
/* set updated flag if one complete packet was parsed */
+ st24_rssi = RC_INPUT_RSSI_MAX;
*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
}
@@ -170,6 +171,12 @@ controls_tick() {
perf_begin(c_gather_dsm);
bool dsm_updated, st24_updated;
(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+ if (dsm_updated) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ }
+ if (st24_updated) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+ }
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
@@ -417,6 +424,15 @@ controls_tick() {
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
override = true;
+ /*
+ if the FMU is dead then enable override if we have a
+ mixer and OVERRIDE_IMMEDIATE is set
+ */
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+ override = true;
+
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9b2e047cbbbccb7bc8d07f4a6c2347dd31346fa1..c7e9ae3eb3fafff3808ae2b8a050da4b6d822071 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,7 @@
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 30f32b38e7d57fafcd8e69310eebfbedaae6accd..14ee9cb405876440f70457a2286709d8952f2726 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
#define NUM_MSG 2
static char msg[NUM_MSG][40];
+static void heartbeat_blink(void);
+static void ring_blink(void);
+
/*
* add a debug message to be printed on the console
*/
@@ -124,8 +127,64 @@ heartbeat_blink(void)
{
static bool heartbeat = false;
LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+ring_blink(void)
+{
#ifdef GPIO_LED4
- LED_RING(heartbeat);
+
+ if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+ /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ LED_RING(1);
+ return;
+ }
+
+ // XXX this led code does have
+ // intentionally a few magic numbers.
+ const unsigned max_brightness = 118;
+
+ static unsigned counter = 0;
+ static unsigned brightness = max_brightness;
+ static unsigned brightness_counter = 0;
+ static unsigned on_counter = 0;
+
+ if (brightness_counter < max_brightness) {
+
+ bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
+
+ // XXX once led is PWM driven,
+ // remove the ! in the line below
+ // to return to the proper breathe
+ // animation / pattern (currently inverted)
+ LED_RING(!on);
+ brightness_counter++;
+
+ if (on) {
+ on_counter++;
+ }
+
+ } else {
+
+ if (counter >= 62) {
+ counter = 0;
+ }
+
+ int n;
+
+ if (counter < 32) {
+ n = counter;
+
+ } else {
+ n = 62 - counter;
+ }
+
+ brightness = (n * n) / 8;
+ brightness_counter = 0;
+ on_counter = 0;
+ counter++;
+ }
+
#endif
}
@@ -300,6 +359,8 @@ user_start(int argc, char *argv[])
heartbeat_blink();
}
+ ring_blink();
+
check_reboot();
/* check for debug activity (default: none) */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 8186e4c78b00d263e258681cd89c7b13b79ae31b..93a33490fa9581c93e636d8cf2a3fab3d47b1f61 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit;
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s))
+#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s))
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49c2a9f56ba64087fe3b5d5e0b7695037912ac06..fbfdd35db0daa0d046ee57ae14d5066b34f0f046 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] =
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN | \
PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
- PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+ PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
+ PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d617c93e71899eec63ec707e599429899b..d76ec55f052f2a9f176b5d6e9204ebe5dbd3227a 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
+#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -122,10 +124,42 @@ sbus_init(const char *device)
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
- char a = 'A';
- write(sbus_fd, &a, 1);
-}
+ uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
+ uint8_t offset = 0;
+ uint16_t value;
+ hrt_abstime now;
+
+ now = hrt_absolute_time();
+
+ if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
+ last_txframe_time = now;
+ uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
+
+ /* 16 is sbus number of servos/channels minus 2 single bit channels.
+ * currently ignoring single bit channels. */
+
+ for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
+ value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
+
+ /*protect from out of bounds values and limit to 11 bits*/
+ if (value > 0x07ff ) {
+ value = 0x07ff;
+ }
+
+ while (offset >= 8) {
+ ++byteindex;
+ offset -= 8;
+ }
+ oframe[byteindex] |= (value << (offset)) & 0xff;
+ oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+ oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+ offset += 11;
+ }
+
+ write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+ }
+}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index a28d43e7255ca363dd2ffa914abd257f74242dd5..d4a00af39d3f22dd54eda40c08dc3dc65bc17aff 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -43,3 +43,5 @@ SRCS = sdlog2.c \
logbuffer.c
MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index 5b1bc5e86f6daf4addb2c431069abd6e51fac947..dfbba92d918a025071cf2250a02bc7a9c15840b6 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -42,3 +42,5 @@ SRCS = sensors.cpp \
sensor_params.c
MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
new file mode 100644
index 0000000000000000000000000000000000000000..4bcf957848a8bc289b4c7adff453a046298e1b53
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mcu_version.c
+ *
+ * Read out the microcontroller version from the board
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include "mcu_version.h"
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_ARCH_CHIP_STM32
+#include <up_arch.h>
+
+#define DBGMCU_IDCODE 0xE0042000
+
+#define STM32F40x_41x 0x413
+#define STM32F42x_43x 0x419
+
+#define REVID_MASK 0xFFFF0000
+#define DEVID_MASK 0xFFF
+
+#endif
+
+
+
+int mcu_version(char* rev, char** revstr)
+{
+#ifdef CONFIG_ARCH_CHIP_STM32
+ uint32_t abc = getreg32(DBGMCU_IDCODE);
+
+ int32_t chip_version = abc & DEVID_MASK;
+ enum MCU_REV revid = (abc & REVID_MASK) >> 16;
+
+ switch (chip_version) {
+ case STM32F40x_41x:
+ *revstr = "STM32F40x";
+ break;
+ case STM32F42x_43x:
+ *revstr = "STM32F42x";
+ break;
+ default:
+ *revstr = "STM32F???";
+ break;
+ }
+
+ switch (revid) {
+
+ case MCU_REV_STM32F4_REV_A:
+ *rev = 'A';
+ break;
+ case MCU_REV_STM32F4_REV_Z:
+ *rev = 'Z';
+ break;
+ case MCU_REV_STM32F4_REV_Y:
+ *rev = 'Y';
+ break;
+ case MCU_REV_STM32F4_REV_1:
+ *rev = '1';
+ break;
+ case MCU_REV_STM32F4_REV_3:
+ *rev = '3';
+ break;
+ default:
+ *rev = '?';
+ revid = -1;
+ break;
+ }
+
+ return revid;
+#else
+ return -1;
+#endif
+}
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b3d0aba9b0ab31a2c741718773cc82117c0796f
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/* magic numbers from reference manual */
+enum MCU_REV {
+ MCU_REV_STM32F4_REV_A = 0x1000,
+ MCU_REV_STM32F4_REV_Z = 0x1001,
+ MCU_REV_STM32F4_REV_Y = 0x1003,
+ MCU_REV_STM32F4_REV_1 = 0x1007,
+ MCU_REV_STM32F4_REV_3 = 0x2001
+};
+
+/**
+ * Reports the microcontroller version of the main CPU.
+ *
+ * @param rev The silicon revision character
+ * @param revstr The full chip name string
+ * @return The silicon revision / version number as integer
+ */
+__EXPORT int mcu_version(char* rev, char** revstr);
diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index bf3428a50bd3bd3483876087eb2e26dd17c88b1d..0d629d61007e1729ceb25fb24af84faaf38580eb 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
warnx("line too long");
+ fclose(fp);
return -1;
}
@@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
strcat(buf, line);
}
+ fclose(fp);
return 0;
}
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 147903aa035b8f2cc869f7d99e5c0f2ac291c888..fe8b7e75a0c20934d0c4dbed39962bdcf0f92081 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,5 +53,7 @@ SRCS = err.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c \
- circuit_breaker.c
+ circuit_breaker.c \
+ mcu_version.c
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 0c29101fec657f13396340cd742309b816aa4868..9385ce253c74e89c53eced2975cf884aa45305b4 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -44,3 +44,5 @@ SRCS = uORB.cpp \
objects_common.cpp \
Publication.cpp \
Subscription.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc3ff9363eea8a54b73e1adc878655f6e..50b7bd9e59f5dbce572fe284b90af7b5cfa89018 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
* Switch position
*/
typedef enum {
- SWITCH_POS_NONE = 0, /**< switch is not mapped */
+ SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
- switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
- switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
- switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
- switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
- switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
- switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-}; /**< manual control inputs */
+ switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+ switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+ switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+ switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
+ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
+ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+};
/**
* @}
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b14ae1edd406bdb12cc45ade5c08fa0056658834..16916cc4df7a7ca1c28c4faae1edd12b29c2c4d6 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -34,6 +34,8 @@
/**
* @file rc_channels.h
* Definition of the rc_channels uORB topic.
+ *
+ * @deprecated DO NOT USE FOR NEW CODE
*/
#ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
AUX_2,
AUX_3,
AUX_4,
- AUX_5,
- RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
+ AUX_5
};
+// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+
+#define RC_CHANNELS_FUNCTION_MAX 18
+
/**
* @addtogroup topics
* @{
diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
index 491096934ae1482b1cddbcedff908ef8aebfa3d7..2dd90e69daec55cf769382f43ba52c4a335446e5 100644
--- a/src/modules/uORB/topics/test_motor.h
+++ b/src/modules/uORB/topics/test_motor.h
@@ -57,7 +57,7 @@
struct test_motor_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
unsigned motor_number; /**< number of motor to spin */
- float value; /**< output data, in natural output units */
+ float value; /**< output power, range [0..1] */
};
/**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a1b2667e393f8072356f5290fb216fc3da9fcd4c..91491c148f9aae4bdbeba2748b69b1f9e066e64d 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -108,7 +108,7 @@ typedef enum {
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
- NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
+ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef22891a55e89e55c56c8c825a25ed72a..1d23099f3d8eaf2e41a401a8daf4e79e8d96b4e7 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
#include "esc.hpp"
#include <systemlib/err.h>
+
+#define MOTOR_BIT(x) (1<<(x))
+
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
@@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
- if (_armed) {
- for (unsigned i = 0; i < num_outputs; i++) {
-
+ for (unsigned i = 0; i < num_outputs; i++) {
+ if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
@@ -111,6 +113,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
+ } else {
+ msg.cmd.push_back(static_cast<unsigned>(0));
}
}
@@ -121,9 +125,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
+{
+ if (arm) {
+ _armed_mask = -1;
+ } else {
+ _armed_mask = 0;
+ }
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
{
- _armed = arm;
+ if (arm) {
+ _armed_mask = MOTOR_BIT(num);
+ } else {
+ _armed_mask = 0;
+ }
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e629db8cf0293c3a283bec21d10e01ba7..12c035542256cf7d2815e22e90b03839bda77c7e 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
void update_outputs(float *outputs, unsigned num_outputs);
- void arm_esc(bool arm);
+ void arm_all_escs(bool arm);
+ void arm_single_esc(int num, bool arm);
private:
/**
@@ -98,6 +99,11 @@ private:
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
+ /*
+ * ESC states
+ */
+ uint32_t _armed_mask = 0;
+
/*
* Perf counters
*/
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828e25010a7d58bbdcaf4afd9660bfea70..8741ae20dd7e4670851e5bd2e9cd133e6b8a0036 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
{
auto report = ::baro_report();
- report.timestamp = msg.getUtcTimestamp().toUSec();
- if (report.timestamp == 0) {
- report.timestamp = msg.getMonotonicTimestamp().toUSec();
- }
-
+ report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = msg.static_temperature;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf4cc2eb951e564c816f35d28a174412c..a375db37f8aab1977425cb8de6857c65545ef90b 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
auto report = ::vehicle_gps_position_s();
- report.timestamp_position = hrt_absolute_time();
+ report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401015c41f633c2c52b634c929eb5f40e8..2111ff80b664093fbe8a81b69940ce8149b205b5 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
#pragma once
-#include <drivers/drv_hrt.h>
-
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f062fa03060028f0af4b2a647626c2f4..35ebee5426ae9e9744d38423290ffdedd8b34745 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,8 @@
#include "mag.hpp"
+#include <systemlib/err.h>
+
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
@@ -49,6 +51,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
_sub_mag(node)
{
+ _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
_scale.z_scale = 1.0F;
@@ -69,9 +73,36 @@ int UavcanMagnetometerBridge::init()
return 0;
}
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+ static uint64_t last_read = 0;
+ struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+ /* buffer must be large enough */
+ unsigned count = buflen / sizeof(struct mag_report);
+ if (count < 1) {
+ return -ENOSPC;
+ }
+
+ if (last_read < _report.timestamp) {
+ /* copy report */
+ lock();
+ *mag_buf = _report;
+ last_read = _report.timestamp;
+ unlock();
+ return sizeof(struct mag_report);
+ } else {
+ /* no new data available, warn caller */
+ return -EAGAIN;
+ }
+}
+
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
+ case SENSORIOCSQUEUEDEPTH: {
+ return OK; // Pretend that this stuff is supported to keep APM happy
+ }
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
@@ -84,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
- return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard
+ return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
@@ -106,18 +137,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
{
- auto report = ::mag_report();
-
- report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
-
- report.timestamp = msg.getUtcTimestamp().toUSec();
- if (report.timestamp == 0) {
- report.timestamp = msg.getMonotonicTimestamp().toUSec();
- }
+ lock();
+ _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
+ _report.timestamp = msg.getMonotonicTimestamp().toUSec();
- report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
- report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
- report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+ _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+ _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+ _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+ unlock();
- publish(msg.getSrcNodeID().get(), &report);
+ publish(msg.getSrcNodeID().get(), &_report);
}
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f714f5a2af9f0fcdffa8ceedc04fde496..74077d883e37e182fc782996ffa8b45a67cf6b77 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -54,6 +54,7 @@ public:
int init() override;
private:
+ ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
mag_scale _scale = {};
+ mag_report _report = {};
};
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 9608ce680451196cd37f05f887ee49e2b9a63533..0999938fc3b05653594657026ac87ceb33c648ff 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
return;
}
+ // update device id as we now know our device node_id
+ _device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 1316f7ecc510fe6d9461c3381fe82f68256dee9b..e31960537003789b4984e1fe30632c8287012d85 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -112,6 +112,8 @@ protected:
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+ _device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+ _device_id.devid_s.bus = 0;
}
/**
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a8485ee44e31de4e29fa13e42a878c6aff678c7e..2c543462ec0a33307986bc61f764eaccc102df21 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -279,6 +279,7 @@ int UavcanNode::run()
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ _test_motor_sub = orb_subscribe(ORB_ID(test_motor));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,13 @@ int UavcanNode::run()
}
// can we mix?
- if (controls_updated && (_mixers != nullptr)) {
+ if (_test_in_progress) {
+ float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
+ test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+
+ // Output to the bus
+ _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+ } else if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
@@ -384,15 +391,27 @@ int UavcanNode::run()
}
}
- // Check arming state
+ // Check motor test state
bool updated = false;
+ orb_check(_test_motor_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+ // Update the test status and check that we're not locked down
+ _test_in_progress = (_test_motor.value > 0);
+ _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+ }
+
+ // Check arming state
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
- // Update the armed status and check that we're not locked down
- bool set_armed = _armed.armed && !_armed.lockdown;
+ // Update the armed status and check that we're not locked down and motor
+ // test is not running
+ bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
arm_actuators(set_armed);
}
@@ -429,7 +448,7 @@ int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
- _esc_controller.arm_esc(arm);
+ _esc_controller.arm_all_escs(arm);
return OK;
}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db97411fd9b11c9c72847b34b871a4d578dce..274321f0dbdd2ee09000c4eb5d38bb928db4bde8 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
@@ -103,6 +104,10 @@ private:
actuator_armed_s _armed; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
+ int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
+ test_motor_s _test_motor;
+ bool _test_in_progress = false;
+
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
index eb36d2ded8cc5edcb9830e4236b7cb21cc4fe048..261428ef94d26fee859602a5e3916dcfab9acc3d 100644
--- a/src/systemcmds/motor_test/module.mk
+++ b/src/systemcmds/motor_test/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND = motor_test
SRCS = motor_test.c
MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION = -Os
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 079f99674a366d246a8bface35414485d43fc093..1b7ff75f7be4c3fa17f5f492f44186b65c88e59e 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -54,7 +54,8 @@
#include "systemlib/err.h"
-__EXPORT int motor_test_main(int argc, char *argv[]);
+__EXPORT int motor_test_main(int argc, char *argv[]);
+
static void motor_test(unsigned channel, float value);
static void usage(const char *reason);
@@ -67,19 +68,20 @@ void motor_test(unsigned channel, float value)
_test_motor.timestamp = hrt_absolute_time();
_test_motor.value = value;
- if (_test_motor_pub > 0) {
- /* publish armed state */
- orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
- } else {
- /* advertise and publish */
- _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
- }
+ if (_test_motor_pub > 0) {
+ /* publish test state */
+ orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+ } else {
+ /* advertise and publish */
+ _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+ }
}
static void usage(const char *reason)
{
- if (reason != NULL)
+ if (reason != NULL) {
warnx("%s", reason);
+ }
errx(1,
"usage:\n"
@@ -90,8 +92,9 @@ static void usage(const char *reason)
int motor_test_main(int argc, char *argv[])
{
- unsigned long channel, lval;
- float value;
+ unsigned long channel = 0;
+ unsigned long lval;
+ float value = 0.0f;
int ch;
if (argc != 5) {
@@ -102,18 +105,18 @@ int motor_test_main(int argc, char *argv[])
switch (ch) {
case 'm':
- /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+ /* Read in motor number */
channel = strtoul(optarg, NULL, 0);
break;
case 'p':
- /* Read in custom low value */
+ /* Read in power value */
lval = strtoul(optarg, NULL, 0);
if (lval > 100)
usage("value invalid");
- value = (float)lval/100.f;
+ value = ((float)lval)/100.f;
break;
default:
usage(NULL);
@@ -122,7 +125,7 @@ int motor_test_main(int argc, char *argv[])
motor_test(channel, value);
- printf("motor %d set to %.2f\n", channel, value);
+ printf("motor %d set to %.2f\n", channel, (double)value);
exit(0);
}
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index f06c49552e41685e6eee554fa9fe1127e2abca6a..edeb5c6248aaa4d2c4ae283ed82106861d77b11f 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -51,6 +51,8 @@
#include <fcntl.h>
#include <systemlib/err.h>
+#include <uORB/topics/actuator_armed.h>
+
__EXPORT int nshterm_main(int argc, char *argv[]);
int
@@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[])
}
unsigned retries = 0;
int fd = -1;
+ int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+ struct actuator_armed_s armed;
+ /* we assume the system does not provide arming status feedback */
+ bool armed_updated = false;
+
+ /* try the first 30 seconds or if arming system is ready */
+ while ((retries < 300) || armed_updated) {
+
+ /* abort if an arming topic is published and system is armed */
+ bool updated = false;
+ if (orb_check(armed_fd, &updated)) {
+ /* the system is now providing arming status feedback.
+ * instead of timing out, we resort to abort bringing
+ * up the terminal.
+ */
+ armed_updated = true;
+ orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+ if (armed.armed) {
+ /* this is not an error, but we are done */
+ exit(0);
+ }
+ }
- /* try the first 30 seconds */
- while (retries < 300) {
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 478c2a772f812acb18ee6131c855af4ed8148a7d..eeba89fa85f812159cf4c8bde786d504552f61d5 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -74,34 +74,34 @@ usage(const char *reason)
"usage:\n"
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n"
- " arm Arm output\n"
- " disarm Disarm output\n"
+ "arm\t\t\t\tArm output\n"
+ "disarm\t\t\t\tDisarm output\n"
"\n"
- " rate ... Configure PWM rates\n"
- " [-g <channel group>] Channel group that should update at the alternate rate\n"
- " [-m <chanmask> ] Directly supply channel mask\n"
- " [-a] Configure all outputs\n"
- " -r <alt_rate> PWM rate (50 to 400 Hz)\n"
+ "rate ...\t\t\tConfigure PWM rates\n"
+ "\t[-g <channel group>]\t(e.g. 0,1,2)\n"
+ "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+ "\t[-a]\t\t\tConfigure all outputs\n"
+ "\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
"\n"
- " failsafe ... Configure failsafe PWM values\n"
- " disarmed ... Configure disarmed PWM values\n"
- " min ... Configure minimum PWM values\n"
- " max ... Configure maximum PWM values\n"
- " [-c <channels>] Supply channels (e.g. 1234)\n"
- " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
- " [-a] Configure all outputs\n"
- " -p <pwm value> PWM value\n"
+ "failsafe ...\t\t\tFailsafe PWM\n"
+ "disarmed ...\t\t\tDisarmed PWM\n"
+ "min ...\t\t\t\tMinimum PWM\n"
+ "max ...\t\t\t\tMaximum PWM\n"
+ "\t[-c <channels>]\t\t(e.g. 1234)\n"
+ "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+ "\t[-a]\t\t\tConfigure all outputs\n"
+ "\t-p <pwm value>\t\tPWM value\n"
"\n"
- " test ... Directly set PWM values\n"
- " [-c <channels>] Supply channels (e.g. 1234)\n"
- " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
- " [-a] Configure all outputs\n"
- " -p <pwm value> PWM value\n"
+ "test ...\t\t\tDirectly set PWM\n"
+ "\t[-c <channels>]\t\t(e.g. 1234)\n"
+ "\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+ "\t[-a]\t\t\tConfigure all outputs\n"
+ "\t-p <pwm value>\t\tPWM value\n"
"\n"
- " info Print information about the PWM device\n"
+ "info\t\t\t\tPrint information\n"
"\n"
- " -v Print verbose information\n"
- " -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+ "\t-v\t\t\tVerbose\n"
+ "\t-d <dev>\t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n"
);
}
@@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[])
unsigned single_ch = 0;
unsigned pwm_value = 0;
- if (argc < 1)
+ if (argc < 2)
usage(NULL);
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
@@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[])
/* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
- usage("bad set_mask value");
+ usage("BAD set_mask VAL");
break;
case 'a':
@@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[])
case 'p':
pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
- usage("bad PWM value provided");
+ usage("BAD PWM VAL");
break;
case 'r':
alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
- usage("bad alternative rate provided");
+ usage("BAD rate VAL");
break;
default:
break;
@@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[])
}
if (print_verbose && set_mask > 0) {
- warnx("Chose channels: ");
+ warnx("Channels: ");
printf(" ");
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
if (set_mask & 1<<i)
@@ -364,7 +364,7 @@ pwm_main(int argc, char *argv[])
usage("no channels set");
}
if (pwm_value == 0)
- usage("no PWM value provided");
+ usage("no PWM provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
@@ -383,7 +383,7 @@ pwm_main(int argc, char *argv[])
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
- errx(ret, "failed setting failsafe values");
+ errx(ret, "BAD input VAL");
}
exit(0);
@@ -393,7 +393,7 @@ pwm_main(int argc, char *argv[])
usage("no channels set");
}
if (pwm_value == 0)
- usage("no PWM value provided");
+ usage("no PWM provided");
/* get current servo values */
struct pwm_output_values last_spos;
@@ -487,7 +487,7 @@ pwm_main(int argc, char *argv[])
steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
steps_timing_index++ ) {
- warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+ warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
@@ -526,7 +526,7 @@ pwm_main(int argc, char *argv[])
err(1, "PWM_SERVO_SET(%d)", i);
}
}
- warnx("Key pressed, user abort\n");
+ warnx("User abort\n");
exit(0);
}
}
@@ -675,7 +675,7 @@ pwm_main(int argc, char *argv[])
exit(0);
}
- usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
+ usage(NULL);
return 0;
}
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 9ae080ee21a6a83ffcb4d9ece52cbbf95f05aeb8..2ead3e63234881497a9579bbf6e7fcd2825cc6c7 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -44,14 +44,16 @@
#include <string.h>
#include <version/version.h>
#include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
-// string constants for version commands
+/* string constants for version commands */
static const char sz_ver_hw_str[] = "hw";
static const char sz_ver_hwcmp_str[] = "hwcmp";
static const char sz_ver_git_str[] = "git";
static const char sz_ver_bdate_str[] = "bdate";
static const char sz_ver_gcc_str[] = "gcc";
static const char sz_ver_all_str[] = "all";
+static const char mcu_ver_str[] = "mcu";
static void usage(const char *reason)
{
@@ -59,55 +61,87 @@ static void usage(const char *reason)
printf("%s\n", reason);
}
- printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+ printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
}
__EXPORT int ver_main(int argc, char *argv[]);
int ver_main(int argc, char *argv[])
{
- int ret = 1; //defaults to an error
+ /* defaults to an error */
+ int ret = 1;
- // first check if there are at least 2 params
+ /* first check if there are at least 2 params */
if (argc >= 2) {
if (argv[1] != NULL) {
- if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
- printf("%s\n", HW_ARCH);
- ret = 0;
- } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+ if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
if (argc >= 3 && argv[2] != NULL) {
- // compare 3rd parameter with HW_ARCH string, in case of match, return 0
+ /* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
if (ret == 0) {
printf("ver hwcmp match: %s\n", HW_ARCH);
}
+ return ret;
} else {
errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
}
+ }
- } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
- printf("%s\n", FW_GIT);
- ret = 0;
+ /* check if we want to show all */
+ bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
- } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
- printf("%s %s\n", __DATE__, __TIME__);
+ if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+ printf("HW arch: %s\n", HW_ARCH);
ret = 0;
- } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
- printf("%s\n", __VERSION__);
+ }
+
+ if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+ printf("FW git-hash: %s\n", FW_GIT);
ret = 0;
- } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
- printf("HW arch: %s\n", HW_ARCH);
+ }
+
+ if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
printf("Build datetime: %s %s\n", __DATE__, __TIME__);
- printf("FW git-hash: %s\n", FW_GIT);
- printf("GCC toolchain: %s\n", __VERSION__);
ret = 0;
- } else {
+ }
+
+ if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+ printf("Toolchain: %s\n", __VERSION__);
+ ret = 0;
+
+ }
+
+ if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
+
+ char rev;
+ char* revstr;
+
+ int chip_version = mcu_version(&rev, &revstr);
+
+ if (chip_version < 0) {
+ printf("UNKNOWN MCU\n");
+
+ } else {
+ printf("MCU: %s, rev. %c\n", revstr, rev);
+
+ if (chip_version < MCU_REV_STM32F4_REV_3) {
+ printf("\nWARNING WARNING WARNING!\n"
+ "Revision %c has a silicon errata\n"
+ "This device can only utilize a maximum of 1MB flash safely!\n"
+ "http://px4.io/help/errata\n\n", rev);
+ }
+ }
+
+ ret = 0;
+ }
+
+ if (ret == 1) {
errx(1, "unknown command.\n");
}