diff options
39 files changed, 538 insertions, 261 deletions
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index cf86dd668..826e12c97 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -1,7 +1,10 @@ # GDB/Python functions for dealing with NuttX +from __future__ import print_function import gdb, gdb.types +parse_int = lambda x: int(str(x), 0) + class NX_register_set(object): """Copy of the registers for a given context""" @@ -155,7 +158,7 @@ class NX_task(object): result = [] for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): entry = pidhash_value[i] - pid = int(entry['pid']) + pid = parse_int(entry['pid']) if pid is not -1: result.append(pid) return result @@ -184,7 +187,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, int(stack_limit)): + for offset in range(0, parse_int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -244,7 +247,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = int(filearray[i]['f_inode']) + inode = parse_int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -299,7 +302,7 @@ class NX_show_task (gdb.Command): super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) def invoke(self, arg, from_tty): - t = NX_task.for_pid(int(arg)) + t = NX_task.for_pid(parse_int(arg)) if t is not None: my_fmt = 'PID:{pid} name:{name} state:{state}\n' my_fmt += ' stack used {stack_used} of {stack_limit}\n' @@ -435,12 +438,12 @@ class NX_tcb(object): 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]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_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] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -469,12 +472,12 @@ class NX_check_stack_order(gdb.Command): 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]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_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] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -488,7 +491,7 @@ class NX_check_stack_order(gdb.Command): def getSPfromTask(self,tcb): regmap = NX_register_set.v7em_regmap a =tcb['xcp']['regs'] - return int(a[regmap['SP']]) + return parse_int(a[regmap['SP']]) def find_closest(self,list,val): tmp_list = [abs(i-val) for i in list] @@ -525,8 +528,8 @@ class NX_check_stack_order(gdb.Command): 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(parse_int(t['stack_alloc_ptr'])) + p.append(parse_int(t['adj_stack_ptr'])) p.append(self.getSPfromTask(t)) stackadresses[str(t['name'])] = p; address = int("0x30000000",0) @@ -594,12 +597,12 @@ class NX_search_tcb(gdb.Command): 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]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_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] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def invoke(self,args,sth): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -612,7 +615,7 @@ class NX_search_tcb(gdb.Command): # filter for tasks that are listed twice tasks_filt = {} for t in tasks: - pid = int(t['pid']); + pid = parse_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))) @@ -653,62 +656,49 @@ class NX_my_bt(gdb.Command): tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer()) return tcb_ptr.dereference() - def print_instruction_at(self,addr,stack_percentage): + def resolve_file_line_func(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 + if words[0] != 'No': 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 - - - + return words[3].strip('"'), line, func def invoke(self,args,sth): - addr_dec = int(args[2:],16) - _tcb = self.get_tcb_from_address(addr_dec) + try: + addr_dec = parse_int(args) # Trying to interpret the input as TCB address + except ValueError: + for task in NX_task.tasks(): # Interpreting as a task name + if task.name == args: + _tcb = task._tcb + break + else: + _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 + up_stack = parse_int(_tcb['adj_stack_ptr']) + curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = parse_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): + item = 0 + for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4): 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 + filename,line,func = self.resolve_file_line_func(mem, stack_percentage) + print('#%-2d ' % item, '0x%08x in ' % mem, func, ' at ', filename, ':', line, sep='') + item += 1 NX_my_bt() diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh new file mode 100755 index 000000000..ab06a1b66 --- /dev/null +++ b/Debug/poor-mans-profiler.sh @@ -0,0 +1,260 @@ +#!/bin/bash +# +# Poor man's sampling profiler for NuttX. +# +# Usage: Install flamegraph.pl in your PATH, configure your .gdbinit, run the script with proper arguments and go +# have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly +# interfere with normal operation of the target, which means that you can't profile real-time tasks with it. +# +# Requirements: ARM GDB with Python support +# + +set -e +root=$(dirname $0)/.. + +function die() +{ + echo "$@" + exit 1 +} + +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} + +which flamegraph.pl > /dev/null || die "Install flamegraph.pl first" + +# +# Parsing the arguments. Read this section for usage info. +# +nsamples=0 +sleeptime=0.1 # Doctors recommend 7-8 hours a day +taskname= +elf=$root/Build/px4fmu-v2_default.build/firmware.elf +append=0 +fgfontsize=10 +fgwidth=1900 + +for i in "$@" +do + case $i in + --nsamples=*) + nsamples="${i#*=}" + ;; + --sleeptime=*) + sleeptime="${i#*=}" + ;; + --taskname=*) + taskname="${i#*=}" + ;; + --elf=*) + elf="${i#*=}" + ;; + --append) + append=1 + ;; + --fgfontsize=*) + fgfontsize="${i#*=}" + ;; + --fgwidth=*) + fgwidth="${i#*=}" + ;; + *) + usage + ;; + esac + shift +done + +# +# Temporary files +# +stacksfile=/tmp/pmpn-stacks.log +foldfile=/tmp/pmpn-folded.txt +graphfile=/tmp/pmpn-flamegraph.svg +gdberrfile=/tmp/pmpn-gdberr.log + +# +# Sampling if requested. Note that if $append is true, the stack file will not be rewritten. +# +cd $root + +if [[ $nsamples > 0 ]] +then + [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") + + echo "Sampling the task '$taskname'..." + + for x in $(seq 1 $nsamples) + do + if [[ "$taskname" = "" ]] + then + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" -ex bt \ + 2> $gdberrfile \ + | sed -n 's/\(#.*\)/\1/p' \ + >> $stacksfile + else + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + fi + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime + done + + echo + echo "Stacks saved to $stacksfile" +else + echo "Sampling skipped - set 'nsamples' to re-sample." +fi + +# +# Folding the stacks. +# +[ -f $stacksfile ] || die "Where are the stack samples?" + +cat << 'EOF' > /tmp/pmpn-folder.py +# +# This stack folder correctly handles C++ types. +# +from __future__ import print_function, division +import fileinput, collections, os, sys + +def enforce(x, msg='Invalid input'): + if not x: + raise Exception(msg) + +def split_first_part_with_parens(line): + LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} + RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + QUOTES = set(['"', "'"]) + quotes = collections.defaultdict(bool) + braces = collections.defaultdict(int) + out = '' + for ch in line: + out += ch + # escape character cancels further processing + if ch == '\\': + continue + # special cases + if out.endswith('operator>') or out.endswith('operator>>') or out.endswith('operator->'): # gotta love c++ + braces['<>'] += 1 + if out.endswith('operator<') or out.endswith('operator<<'): + braces['<>'] -= 1 + # switching quotes + if ch in QUOTES: + quotes[ch] = not quotes[ch] + # counting parens only when outside quotes + if sum(quotes.values()) == 0: + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 + # sanity check + for v in braces.values(): + enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) + # termination condition + if ch == ' ' and sum(braces.values()) == 0: + break + out = out.strip() + return out, line[len(out):] + +def parse(line): + def take_path(line, output): + line = line.strip() + if line.startswith('at '): + line = line[3:].strip() + if line: + output['file_full_path'] = line.rsplit(':', 1)[0].strip() + output['file_base_name'] = os.path.basename(output['file_full_path']) + output['line'] = int(line.rsplit(':', 1)[1]) + return output + + def take_args(line, output): + line = line.lstrip() + if line[0] == '(': + output['args'], line = split_first_part_with_parens(line) + return take_path(line.lstrip(), output) + + def take_function(line, output): + output['function'], line = split_first_part_with_parens(line.lstrip()) + return take_args(line.lstrip(), output) + + def take_mem_loc(line, output): + line = line.lstrip() + if line.startswith('0x'): + end = line.find(' ') + num = line[:end] + output['memloc'] = int(num, 16) + line = line[end:].lstrip() + end = line.find(' ') + enforce(line[:end] == 'in') + line = line[end:].lstrip() + return take_function(line, output) + + def take_frame_num(line, output): + line = line.lstrip() + enforce(line[0] == '#') + end = line.find(' ') + num = line[1:end] + output['frame_num'] = int(num) + return take_mem_loc(line[end:], output) + + return take_frame_num(line, {}) + +stacks = collections.defaultdict(int) +current = '' + +stack_tops = collections.defaultdict(int) +num_stack_frames = 0 + +for idx,line in enumerate(fileinput.input()): + try: + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + + if inf['frame_num'] == 0: + num_stack_frames += 1 + stack_tops[fun] += 1 + elif current: + stacks[current] += 1 + current = '' + except Exception, ex: + print('ERROR (line %d):' % (idx + 1), ex, file=sys.stderr) + +for s, f in sorted(stacks.items(), key=lambda (s, f): s): + print(s, f) + +print('Total stack frames:', num_stack_frames, file=sys.stderr) +print('Top consumers (distribution of the stack tops):', file=sys.stderr) +for name,num in sorted(stack_tops.items(), key=lambda (name, num): num, reverse=True)[:10]: + print('% 5.1f%% ' % (100 * num / num_stack_frames), name, file=sys.stderr) +EOF + +cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile + +echo "Folded stacks saved to $foldfile" + +# +# Graphing. +# +cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile +echo "FlameGraph saved to $graphfile" + +# On KDE, xdg-open prefers Gwenview by default, which doesn't handle interactive SVGs, so we need a browser. +# The current implementation is hackish and stupid. Somebody, please do something about it. +opener=xdg-open +which firefox > /dev/null && opener=firefox +which google-chrome > /dev/null && opener=google-chrome + +$opener $graphfile diff --git a/NuttX b/NuttX -Subproject 574bac488f384ddaa344378e25653c27124a2b6 +Subproject e4c914e261d2647e44d05222afa7aa3cc90d3c6 diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 25f31a7e0..a14eb5247 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 4 -t + sdlog2 start -r 40 -a -b 3 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 22 -t diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a087..c932a6758 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -80,6 +80,7 @@ export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python export OPENOCD = openocd +export GREP = grep # # Host-specific paths, hacks and fixups diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d7e48f03b..d4d73fb84 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -80,13 +80,20 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ - -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ - -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +# Enabling stack checks if OS was build with them +# +TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h +TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 +ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) +ifeq ("$(ENABLE_STACK_CHECKS)","0") +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +endif # Pick the right set of flags for the architecture. # @@ -105,7 +112,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs index c1f5a8ac4..3808fc1cf 100644 --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 317194f68..c44b074f3 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -94,7 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 5e28f2473..4e08d28a2 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index fade1f0c6..539634e3d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140..5a1d5af2c 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ac3dbe5da..dedebdfa0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 @@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index b4f5577ae..74f38c0cb 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 51420eb23..287466db6 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 05dfc99b7..8157bc7e6 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); - default: warnx("ERR: baudrate: %d\n", baud); return -EINVAL; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2a10b0063..95fbed0ba 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,7 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> #include <drivers/device/ringbuffer.h> +#include <drivers/drv_device.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -725,6 +726,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -1305,9 +1309,9 @@ struct hmc5883_bus_option { uint8_t busnum; HMC5883 *dev; } bus_options[] = { - { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, #ifdef PX4_I2C_BUS_ONBOARD - { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, #endif #ifdef PX4_SPIDEV_HMC { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 782ea62fe..f86c1af6b 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -53,6 +53,7 @@ #include <drivers/device/i2c.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "hmc5883.h" @@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus) HMC5883_I2C::HMC5883_I2C(int bus) : I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_I2C::~HMC5883_I2C() @@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: ret = -EINVAL; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 25a2f2b40..aec990ca8 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -53,6 +53,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "hmc5883.h" #include <board_config.h> @@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus) HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_SPI::~HMC5883_SPI() @@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: { ret = -EINVAL; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 436672040..112c01513 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2015 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 @@ -1637,12 +1637,15 @@ sensor_reset(int ms) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) - err(1, "servo arm failed"); + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) { + warnx("sensor rail reset failed"); + } + close(fd); } void diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 96ebedd83..556eebab6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -834,7 +834,7 @@ PX4IO::init() _task = task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 2000, + 1800, (main_t)&PX4IO::task_main_trampoline, nullptr); @@ -1102,7 +1102,7 @@ PX4IO::io_set_control_state(unsigned group) uint16_t regs[_max_actuators]; /* get controls */ - bool changed; + bool changed = false; switch (group) { case 0: @@ -1144,11 +1144,13 @@ PX4IO::io_set_control_state(unsigned group) break; } - if (!changed) + if (!changed) { return -1; + } - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { regs[i] = FLOAT_TO_REG(controls.control[i]); + } /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); @@ -1773,12 +1775,12 @@ PX4IO::print_debug() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 int io_fd = -1; - if (io_fd < 0) { + if (io_fd <= 0) { io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ - if (io_fd > 0) { + if (io_fd >= 0) { pollfd fds[1]; fds[0].fd = io_fd; fds[0].events = POLLIN; @@ -2278,6 +2280,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + break; case PWM_SERVO_SET_FORCE_SAFETY_OFF: /* force safety swith off */ @@ -2853,10 +2856,10 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - printf("check CRC failed - %d\n", ret); + printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } - printf("CRCs match\n"); + printf("[PX4IO::checkcrc] CRCs match\n"); exit(0); } @@ -3003,11 +3006,14 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 2000); + if (poll(fds, 1, 2000) < 0) { + errx(1, "poll fail"); + } if (fds[0].revents == POLLIN) { - int c; - read(0, &c, 1); + /* control logic is to cancel with any key */ + char c; + (void)read(0, &c, 1); if (cancels-- == 0) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ @@ -3069,12 +3075,13 @@ lockdown(int argc, char *argv[]) if (ret > 0) { - read(0, &c, 1); + if (read(0, &c, 1) > 0) { - if (c != 'y') { - exit(0); - } else if (c == 'y') { - break; + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } } } @@ -3237,7 +3244,13 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "limit")) { if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); + int limitrate = atoi(argv[2]); + + if (limitrate > 0) { + g_dev->set_update_rate(limitrate); + } else { + errx(1, "invalid rate: %d", limitrate); + } } else { errx(1, "missing argument (50 - 500 Hz)"); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f..02e527695 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -621,6 +621,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); + up_udelay(100*1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; diff --git a/src/lib/uavcan b/src/lib/uavcan -Subproject c4c45b995f5c8192c7a36c4293c201711ceac74 +Subproject 8966f970135fb421db31886d6f99ac918594a78 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 e61204e6c..b0086676a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; - // compute secondary attitude - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R_adapted = R; - - //move z to x - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); - //move x to z - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); - - //change direction of pitch (convert to right handed system) - R_adapted(0, 0) = -R_adapted(0, 0); - R_adapted(1, 0) = -R_adapted(1, 0); - R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation - euler_angles_sec = R_adapted.to_euler(); - - att.roll_sec = euler_angles_sec(0); - att.pitch_sec = euler_angles_sec(1); - att.yaw_sec = euler_angles_sec(2); - - memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); - - att.rollspeed_sec = -x_aposteriori[2]; - att.pitchspeed_sec = x_aposteriori[1]; - att.yawspeed_sec = x_aposteriori[0]; - att.rollacc_sec = -x_aposteriori[5]; - att.pitchacc_sec = x_aposteriori[4]; - att.yawacc_sec = x_aposteriori[3]; - - att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); - att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); - att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1..d158d7a49 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 EXTRACXXFLAGS = -Wframe-larger-than=2400 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3c71195fb..247a2c5b8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1340,7 +1340,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1457,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("subsystem changed: %d\n", (int)info.subsystem_type); + //warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9d2abd3ce..53013fdb9 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -150,54 +150,60 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - calibration_counter = 0; + calibration_counter = 0U; - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(fds, 1, 1000); - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - calibration_counter++; + calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } + + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -205,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 705e54be3..68bf12024 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -793,7 +793,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b062097fb..15d018ab6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -181,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z - states[13]; + + Vector3f dVelIMURel; + + dVelIMURel.x = dVelIMU.x; + dVelIMURel.y = dVelIMU.y; + dVelIMURel.z = dVelIMU.z - states[13]; delAngTotal.x += correctedDelAng.x; delAngTotal.y += correctedDelAng.y; @@ -263,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; // calculate the magnitude of the nav acceleration (required for GPS // variance estimation) @@ -340,9 +343,9 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; } // square all sigmas @@ -1176,7 +1179,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i<=21; i++) + for (uint8_t i = 16; i < n_states; i++) { Kfusion[i] = 0; } @@ -1357,7 +1360,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1535,14 +1538,14 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = 0.0f; } @@ -1559,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k<=21; k++) + for (uint8_t k = 16; k < n_states; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1661,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1673,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j <= 22; j++) + for (uint8_t j=0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1690,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1702,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1719,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1833,7 +1836,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); // calculate observation jacobians for X LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[0][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0; H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); @@ -1874,7 +1877,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[0][i] = 0.0f; } } @@ -1895,7 +1898,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; // Calculate observation jacobians for Y LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[1][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0; H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; @@ -1936,7 +1939,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[1][i] = 0.0f; } } @@ -1980,7 +1983,7 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j <= 21; j++) + for (uint8_t j = 10; j < n_states; j++) { KH[i][j] = 0.0f; } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58c9429b6..d20f776d6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; + if (!(num_values) || !(values) || !(frame_len)) { + return result; + } + /* avoid racing with PPM updates */ irqstate_t state = irqsave(); @@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) if (*num_values > PX4IO_RC_INPUT_CHANNELS) *num_values = PX4IO_RC_INPUT_CHANNELS; - for (unsigned i = 0; i < *num_values; i++) { + for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; } @@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) ppm_last_valid_decode = 0; /* store PPM frame length */ - if (num_values) - *frame_len = ppm_frame_length; + *frame_len = ppm_frame_length; /* good if we got any channels */ result = (*num_values > 0); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0cc193b6c..a3407e42c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1437,6 +1437,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ATTITUDE --- */ if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.q_w = buf.att.q[0]; + log_msg.body.log_ATT.q_x = buf.att.q[1]; + log_msg.body.log_ATT.q_y = buf.att.q[2]; + log_msg.body.log_ATT.q_z = buf.att.q[3]; log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; @@ -1447,18 +1451,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); - // secondary attitude - log_msg.msg_type = LOG_ATT2_MSG; - log_msg.body.log_ATT.roll = buf.att.roll_sec; - log_msg.body.log_ATT.pitch = buf.att.pitch_sec; - log_msg.body.log_ATT.yaw = buf.att.yaw_sec; - log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; - log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; - log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; - log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; - log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; - log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; - LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c9221d58a..7080d9c31 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,8 +50,11 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 -#define LOG_ATT2_MSG 41 struct log_ATT_s { + float q_w; + float q_x; + float q_y; + float q_z; float roll; float pitch; float yaw; @@ -460,7 +463,7 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c798af3b..793b7c2b6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -1465,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced) /* this sensor is optional, abort without error */ - if (fd > 0) { + if (fd >= 0) { struct airspeed_scale airscale = { _parameters.diff_pres_offset_pa, 1.0f, diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 998b5ac7d..a1a8e7ea8 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -154,6 +154,7 @@ warn(const char *fmt, ...) va_start(args, fmt); vwarn(fmt, args); + va_end(args); } void @@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...) va_start(args, fmt); vwarnc(errcode, fmt, args); + va_end(args); } void @@ -184,6 +186,7 @@ warnx(const char *fmt, ...) va_start(args, fmt); vwarnx(fmt, args); + va_end(args); } void diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 77324415a..019944dc0 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -79,24 +79,6 @@ struct vehicle_attitude_s { float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ - - // secondary attitude, use for VTOL - float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ - float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ - float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ - float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ - float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ - float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ - float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ - float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ - float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ - float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ - float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q_sec[4]; /**< Quaternion (NED) */ - float g_comp_sec[3]; /**< Compensated gravity vector */ - bool R_valid_sec; /**< Rotation matrix valid */ - bool q_valid_sec; /**< Quaternion valid */ - }; /** diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 149b8f6d2..3373aac83 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (Cc) 2012-2015 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 @@ -230,7 +230,7 @@ ORBDevNode::open(struct file *filp) ret = CDev::open(filp); if (ret != OK) - free(sd); + delete sd; return ret; } @@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[]) namespace { -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - /** * Advertise a node; don't consider it an error if the node has * already been advertised. diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e1..51589e43e 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,6 +49,13 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } } UavcanEscController::~UavcanEscController() diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c4..924b730a2 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = uavcan -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -O3 # Main SRCS += uavcan_main.cpp \ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c7..4dc03b61b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -81,6 +81,18 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (res < 0) { std::abort(); } + + if (_perfcnt_node_spin_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed"); + } + + if (_perfcnt_esc_mixer_output_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed"); + } + + if (_perfcnt_esc_mixer_total_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); + } } UavcanNode::~UavcanNode() @@ -118,6 +130,10 @@ UavcanNode::~UavcanNode() } _instance = nullptr; + + perf_free(_perfcnt_node_spin_elapsed); + perf_free(_perfcnt_esc_mixer_output_elapsed); + perf_free(_perfcnt_esc_mixer_total_elapsed); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -265,10 +281,12 @@ int UavcanNode::init(uavcan::NodeID node_id) void UavcanNode::node_spin_once() { + perf_begin(_perfcnt_node_spin_elapsed); const int spin_res = _node.spin(uavcan::MonotonicTime()); if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } /* @@ -344,8 +362,12 @@ int UavcanNode::run() // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); + perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + perf_begin(_perfcnt_esc_mixer_total_elapsed); + (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking @@ -430,7 +452,9 @@ int UavcanNode::run() } // Output to the bus _outputs.timestamp = hrt_absolute_time(); + perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + perf_end(_perfcnt_esc_mixer_output_elapsed); } @@ -484,7 +508,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } - return ::close(_armed_sub); + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } int diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 98f2e5ad4..19b9b4b48 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include <nuttx/config.h> - #include <uavcan_stm32/uavcan_stm32.hpp> #include <drivers/device/device.h> +#include <systemlib/perf_counter.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -66,7 +66,7 @@ */ class UavcanNode : public device::CDev { - static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why static constexpr unsigned RxQueueLenPerIface = 64; static constexpr unsigned StackSize = 3000; @@ -107,11 +107,11 @@ private: int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver int _armed_sub = -1; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system + 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; + test_motor_s _test_motor = {}; bool _test_in_progress = false; unsigned _output_count = 0; ///< number of actuators currently available @@ -135,11 +135,15 @@ private: unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic - uint8_t _actuator_direct_poll_fd_num; - actuator_direct_s _actuator_direct; + uint8_t _actuator_direct_poll_fd_num = 0; + actuator_direct_s _actuator_direct = {}; - actuator_outputs_s _outputs; + actuator_outputs_s _outputs = {}; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + + perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); + perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); + perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); }; |