diff --git a/.travis.yml b/.travis.yml index 9386ddaa31..a994d34715 100644 --- a/.travis.yml +++ b/.travis.yml @@ -52,20 +52,17 @@ script: - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 -# We use an encrypted env variable to ensure this only executes when artifacts are uploaded. -#after_script: -# - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT -# - git log -n1 > $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - echo "Files available at:" >> $PX4_REPORT -# - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT -# - echo "Description of desired tests is available at:" >> $PX4_REPORT -# - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - echo "Thanks for testing!" >> $PX4_REPORT -# - echo " " >> $PX4_REPORT -# - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT" -# - s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ +after_script: + - git clone git://github.com/PX4/CI-Tools.git + - ./CI-Tools/s3cmd-configure +# upload newest build for this branch with s3 index + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ +# archive newest build by date with s3 index + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ +# upload top level index.html and timestamp.html + - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html + - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html deploy: provider: releases @@ -78,18 +75,6 @@ deploy: all_branches: true repo: PX4/Firmware -addons: - artifacts: - paths: - - "Firmware.zip" - key: - secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM= - secret: - secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= - bucket: px4-travis - region: us-east-1 - endpoint: s3-website-us-east-1.amazonaws.com - notifications: webhooks: urls: diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index cf86dd668f..826e12c97c 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 0000000000..ab06a1b66a --- /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/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg new file mode 100644 index 0000000000..e66e1b1cdd Binary files /dev/null and b/Documentation/px4_block_diagram.odg differ diff --git a/Makefile b/Makefile index c727c9a697..af93504eef 100644 --- a/Makefile +++ b/Makefile @@ -169,16 +169,18 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) .NOTPARALLEL: endif +J?=1 + $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -195,11 +197,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff06..4d337171a9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -5,6 +5,11 @@ # NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. # +# +# Start CDC/ACM serial driver +# +sercon + # # Default to auto-start mode. # @@ -43,29 +48,8 @@ else fi unset FRC -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - if sercon - then - echo "[i] USB interface connected" - fi - - echo "[i] Running rc.APM" - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi - if [ $MODE == autostart ] then - echo "[i] AUTOSTART mode" - - # - # Start CDC/ACM serial driver - # - sercon - # Try to get an USB console nshterm /dev/ttyACM0 & diff --git a/Tools/make_color.sh b/Tools/make_color.sh new file mode 100755 index 0000000000..81316a9328 --- /dev/null +++ b/Tools/make_color.sh @@ -0,0 +1,36 @@ +#!/bin/sh +# make_color.sh +# +# Author: Simon Wilks (simon@uaventure.com) +# +# A compiler color coder. +# +# To invoke this script everytime you run make simply create the alias: +# +# alias make='/Tools/make_color.sh' +# +# Color codes: +# +# white "\033[1,37m" +# yellow "\033[1,33m" +# green "\033[1,32m" +# blue "\033[1,34m" +# cyan "\033[1,36m" +# red "\033[1,31m" +# magenta "\033[1,35m" +# black "\033[1,30m" +# darkwhite "\033[0,37m" +# darkyellow "\033[0,33m" +# darkgreen "\033[0,32m" +# darkblue "\033[0,34m" +# darkcyan "\033[0,36m" +# darkred "\033[0,31m" +# darkmagenta "\033[0,35m" +# off "\033[0,0m" +# +OFF="\o033[0m" +WARN="\o033[1;33m" +ERROR="\o033[1;31m" +INFO="\o033[0;37m" + +make ${@} 2>&1 | sed "s/make\[[0-9]\].*/$INFO & $OFF/;s/.*: warning: .*/$WARN & $OFF/;s/.*: error: .*/$ERROR & $OFF/" diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 7eb090d732..4e528f8cdf 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -62,6 +62,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/land_detector # # Estimation modules (EKF / other filters) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ce7388446..5d6beffd7e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/gps # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +# MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +# MODULES += drivers/frsky_telemetry MODULES += modules/sensors -MODULES += drivers/mkblctrl +# MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +# MODULES += modules/uavcan MODULES += modules/land_detector # @@ -121,7 +121,7 @@ MODULES += platforms/nuttx # # OBC challenge # -MODULES += modules/bottle_drop +# MODULES += modules/bottle_drop # # Demo apps diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 435c240bfa..351b9f1b71 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -82,6 +82,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 3969804535..dc8d3f7125 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -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 c1f5a8ac42..3808fc1cfd 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 317194f685..c44b074f31 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 5e28f2473d..4e08d28a29 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-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140d..5a1d5af2c4 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/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index b4f5577aed..74f38c0cb4 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 51420eb23d..287466db60 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/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2b5fef4d77..92b752a282 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -91,6 +91,7 @@ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -171,11 +172,13 @@ private: uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables + bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID + uint64_t _start_time; ///< system time we first attempt to communicate with battery }; namespace @@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _enabled(false), _work{}, _reports(nullptr), _batt_topic(-1), - _batt_orb_id(nullptr) + _batt_orb_id(nullptr), + _start_time(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + // capture startup time + _start_time = hrt_absolute_time(); } BATT_SMBUS::~BATT_SMBUS() @@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg) void BATT_SMBUS::cycle() { + // get current time + uint64_t now = hrt_absolute_time(); + + // exit without rescheduling if we have failed to find a battery after 10 seconds + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + warnx("did not find smart battery"); + return; + } + // read data from sensor struct battery_status_s new_report; // set time of reading - new_report.timestamp = hrt_absolute_time(); + new_report.timestamp = now; // read voltage uint16_t tmp; @@ -375,6 +392,9 @@ BATT_SMBUS::cycle() // notify anyone waiting for data poll_notify(POLLIN); + + // record we are working + _enabled = true; } // schedule a fresh cycle call when the measurement is done diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f588..12d51aeaad 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -45,6 +45,7 @@ #include "drv_orb_dev.h" #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, @@ -67,6 +68,8 @@ struct range_finder_report { float minimum_distance; /**< minimum distance the sensor can measure */ float maximum_distance; /**< maximum distance the sensor can measure */ uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ + float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ + uint8_t just_updated; /** number of the most recent measurement sensor */ }; /** diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 95fbed0ba3..a06be72d9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { return false; + } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1,"Failed to setup poll rate"); } + close(fd); return true; } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf5686583..a4fb7bcc20 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -34,6 +34,7 @@ /** * @file mb12xx.cpp * @author Greg Hulands + * @author Jon Verbeke * * Driver for the Maxbotix sonar range finders connected via I2C. */ @@ -54,6 +55,7 @@ #include #include #include +#include #include #include @@ -72,7 +74,7 @@ #include /* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define MB12XX_DEVICE_PATH "/dev/mb12xx" @@ -83,10 +85,12 @@ #define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,12 +137,19 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + /** * Test whether the device supported by the driver is present at a * specific address. * * @param address The I2C bus address to probe. - * @return True if the device is present. + * @return True if the device is present. */ int probe_address(uint8_t address); @@ -178,7 +189,7 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + { - // enable debug() calls + /* enable debug() calls */ _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... + /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); } @@ -223,7 +238,7 @@ MB12XX::~MB12XX() unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); @@ -242,6 +257,9 @@ MB12XX::init() /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); + _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + if (_reports == nullptr) { goto out; } @@ -250,16 +268,51 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); + struct range_finder_report rf_report = {}; + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } } + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = MB12XX_BASEADDR; + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = MB12XX_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -325,11 +378,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_cycling_rate); /* if we need to start the poll state machine, do it */ if (want_start) { start(); + } return OK; @@ -341,10 +395,10 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_cycling_rate)) { return -EINVAL; } @@ -414,6 +468,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct range_finder_report); struct range_finder_report *rbuf = reinterpret_cast(buffer); int ret = 0; @@ -453,7 +508,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); + usleep(_cycling_rate * 2); /* run the collection phase */ if (OK != collect()) { @@ -474,17 +529,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int MB12XX::measure() { + int ret; /* * Send the command to begin a measurement. */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + debug("i2c::transfer returned %d", ret); return ret; } @@ -506,7 +563,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); 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; @@ -519,7 +576,39 @@ MB12XX::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; + + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (addr_ind.size() == 1) { + report.distance = si_units; + + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { + report.distance_vector[i] = 0; + } + + report.just_updated = 0; + + } else { + /* for multiple sonars connected */ + + /* don't use the orginial single sonar variable */ + report.distance = 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + } + + /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ + report.just_updated = _index_counter; + + /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { + report.distance_vector[addr_ind.size() + i] = 0; + } + } + report.minimum_distance = get_minimum_distance(); report.maximum_distance = get_maximum_distance(); report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; @@ -545,12 +634,13 @@ MB12XX::collect() void MB12XX::start() { + /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ struct subsystem_info_s info = { @@ -564,8 +654,10 @@ MB12XX::start() if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } @@ -578,21 +670,24 @@ MB12XX::stop() void MB12XX::cycle_trampoline(void *arg) { + MB12XX *dev = (MB12XX *)arg; dev->cycle(); + } void MB12XX::cycle() { - /* collection phase? */ if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + set_address(_index_counter); /* perform collection */ if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ + debug("collection error"); + /* if error restart the measurement state machine */ start(); return; } @@ -600,25 +695,37 @@ MB12XX::cycle() /* next phase is measurement */ _collect_phase = false; - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - + _measure_ticks - USEC2TICK(_cycling_rate)); return; } } - /* measurement phase */ + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ if (OK != measure()) { - log("measure error"); + debug("measure error sonar adress %d", _index_counter); } /* next phase is collection */ @@ -629,7 +736,8 @@ MB12XX::cycle() &_work, (worker_t)&MB12XX::cycle_trampoline, this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + USEC2TICK(_cycling_rate)); + } void @@ -750,7 +858,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -779,7 +887,12 @@ test() } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); + } + warnx("time: %lld", report.timestamp); } @@ -830,7 +943,7 @@ info() exit(0); } -} // namespace +} /* namespace */ int mb12xx_main(int argc, char *argv[]) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea91..2be7582442 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,26 +921,50 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + return 1; + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 0000000000..b391b1f6a8 --- /dev/null +++ b/src/drivers/px4flow/i2c_frame.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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 i2c_frame.h + * Definition of i2c frames. + * @author Thomas Boehm + * @author James Goppert + */ + +#ifndef I2C_FRAME_H_ +#define I2C_FRAME_H_ +#include + + +typedef struct i2c_frame +{ + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +} i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +typedef struct i2c_integral_frame +{ + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) + +#endif /* I2C_FRAME_H_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f81..bb0cdbbb6e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -93,38 +93,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -struct i2c_frame { - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; -}; +#include "i2c_frame.h" + struct i2c_frame f; - -struct i2c_integral_frame { - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t time_since_last_sonar_update; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; -} __attribute__((packed)); struct i2c_integral_frame f_integral; - class PX4FLOW: public device::I2C { public: @@ -150,8 +123,7 @@ private: RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; - + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; @@ -261,10 +233,10 @@ out: int PX4FLOW::probe() { - uint8_t val[22]; + uint8_t val[I2C_FRAME_SIZE]; // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address + // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address // 0. The ll40ls gives an error for that, whereas the flow // happily returns some data if (transfer(nullptr, 0, &val[0], 22) != OK) { @@ -469,16 +441,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[47] = { 0 }; + uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { @@ -489,46 +461,12 @@ PX4FLOW::collect() } if (PX4FLOW_REG == 0) { - f.frame_count = val[1] << 8 | val[0]; - f.pixel_flow_x_sum = val[3] << 8 | val[2]; - f.pixel_flow_y_sum = val[5] << 8 | val[4]; - f.flow_comp_m_x = val[7] << 8 | val[6]; - f.flow_comp_m_y = val[9] << 8 | val[8]; - f.qual = val[11] << 8 | val[10]; - f.gyro_x_rate = val[13] << 8 | val[12]; - f.gyro_y_rate = val[15] << 8 | val[14]; - f.gyro_z_rate = val[17] << 8 | val[16]; - f.gyro_range = val[18]; - f.sonar_timestamp = val[19]; - f.ground_distance = val[21] << 8 | val[20]; - - f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; - f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; - f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; - f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; - f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; - f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; - f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; - f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 - | val[39] << 8 | val[38]; - f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.gyro_temperature = val[45] << 8 | val[44]; - f_integral.qual = val[46]; + memcpy(&f, val, I2C_FRAME_SIZE); + memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; - f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; - f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.gyro_temperature = val[23] << 8 | val[22]; - f_integral.qual = val[24]; + memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } @@ -544,7 +482,7 @@ PX4FLOW::collect() report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; @@ -828,7 +766,7 @@ test() warnx("ground_distance: %0.2f m", (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0451bbd1bd..8ce548571d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2860,10 +2860,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); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f6..02e527695c 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 index c4c45b995f..7719f7692a 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d +Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1d..d158d7a49c 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/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f4e3dc4417..2e28a6d2c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) /* Use distance value for range finder report */ struct range_finder_report r; - memset(&r, 0, sizeof(f)); + memset(&r, 0, sizeof(r)); r.timestamp = hrt_absolute_time(); r.error_count = 0; 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 562033db1d..f20bba52e1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -35,6 +35,10 @@ * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * Publication for the desired attitude tracking: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. + * * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin 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 bf65d2805a..60682fb8ee 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,6 +35,12 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * + * Original publication for the desired attitude generation: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 + * + * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1 + * * The controller has two loops: P loop for position error and PID loop for velocity error. * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). @@ -727,11 +733,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b7..9bc9be245c 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,17 +58,17 @@ static const int ERROR = -1; Geofence::Geofence() : - SuperBlock(NULL, "GF"), - _fence_pub(-1), - _altitude_min(0), - _altitude_max(0), - _verticesCount(0), - _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE"), - _param_counter_threshold(this, "COUNT"), - _outside_counter(0), - _mavlinkFd(-1) + SuperBlock(NULL, "GF"), + _fence_pub(-1), + _altitude_min(0), + _altitude_max(0), + _verticesCount(0), + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +{ updateParams(); if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + (double)gps_position.alt * 1.0e-3); } + } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position, baro_altitude_amsl); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + baro_altitude_amsl); } } } @@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude) _outside_counter = 0; return inside_fence; } { + _outside_counter++; - if(_outside_counter > _param_counter_threshold.get()) { + + if (_outside_counter > _param_counter_threshold.get()) { return inside_fence; + } else { return true; } @@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) { return true; + } if (valid()) { @@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } // skip vertex 0 (return point) if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; } } return c; + } else { /* Empty fence --> accept all points */ return true; @@ -188,8 +198,9 @@ bool Geofence::valid() { // NULL fence is valid - if (isEmpty()) + if (isEmpty()) { return true; + } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { @@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[]) return; } - if (argc < 3) + if (argc < 3) { errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + } ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) + + if (ix >= DM_KEY_FENCE_POINTS_MAX) { errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; + } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) + if (last) { publishFence((unsigned)ix + 1); + } + return; } @@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[]) void Geofence::publishFence(unsigned vertices) { - if (_fence_pub == -1) + if (_fence_pub == -1) { _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else + + } else { orb_publish(ORB_ID(fence), _fence_pub, &vertices); + } } int @@ -257,26 +277,29 @@ Geofence::loadFromFile(const char *filename) int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; + int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { return ERROR; } /* create geofence points from valid lines and store in DM */ for (;;) { - /* get a line, bail on error/EOF */ - if (fgets(line, sizeof(line), fp) == NULL) + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* Trim leading whitespace */ size_t textStart = 0; - while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { @@ -299,55 +322,56 @@ Geofence::loadFromFile(const char *filename) if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); - return ERROR; + goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); - vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; - vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; + vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); - return ERROR; + goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + goto error; + } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; + } else { /* Parse the line as the vertical limits */ - if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) - return ERROR; - + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } - - } - fclose(fp); - /* Check if import was successful */ - if(gotVertical && pointCounter > 0) - { + if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); + rc = OK; + } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } - return ERROR; +error: + fclose(fp); + return rc; } int Geofence::clearDm() 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 6de283396c..ec297e7f0c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -877,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -907,6 +908,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -991,6 +993,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b7f6509d73..5387b7e87f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +/** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); + /** * Z axis weight for vision * @@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index d0a65e42ea..51bbda412a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 4bcf957848..24f4e42076 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -47,7 +47,8 @@ #ifdef CONFIG_ARCH_CHIP_STM32 #include -#define DBGMCU_IDCODE 0xE0042000 +#define DBGMCU_IDCODE 0xE0042000 //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code) +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define STM32F40x_41x 0x413 #define STM32F42x_43x 0x419 @@ -57,7 +58,13 @@ #endif - +/** Copy the 96bit MCU Unique ID into the provided pointer */ +void mcu_unique_id(uint32_t *uid_96_bit) +{ + uid_96_bit[0] = getreg32(UNIQUE_ID); + uid_96_bit[1] = getreg32(UNIQUE_ID+4); + uid_96_bit[2] = getreg32(UNIQUE_ID+8); +} int mcu_version(char* rev, char** revstr) { diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index 1b3d0aba9b..c8a0bf5cda 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -33,6 +33,8 @@ #pragma once +#include + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -42,6 +44,15 @@ enum MCU_REV { MCU_REV_STM32F4_REV_3 = 0x2001 }; + +/** + * Reports the microcontroller unique id. + * + * This ID is guaranteed to be unique for every mcu. + * @param uid_96_bit A uint32_t[3] array to copy the data to. + */ +__EXPORT void mcu_unique_id(uint32_t *uid_96_bit); + /** * Reports the microcontroller version of the main CPU. * diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e16..51589e43eb 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 e5d30f6c47..600cb47f39 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 \ @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c75..4dc03b61b7 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 98f2e5ad4b..19b9b4b484 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include - #include #include +#include #include #include @@ -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"); }; diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 077bc47c90..9f13edb184 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -36,7 +36,7 @@ * @author Lorenz Meier * @author Julian Oes * - * config tool. + * config tool. Takes the device name as the first parameter. */ #include @@ -71,18 +71,18 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "gyro")) { - do_gyro(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "accel")) { - do_accel(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "mag")) { - do_mag(argc - 2, argv + 2); + if (!strncmp(argv[1], "/dev/gyro",9)) { + do_gyro(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/accel",10)) { + do_accel(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/mag",8)) { + do_mag(argc - 1, argv + 1); } else { do_device(argc - 1, argv + 1); } } - errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } static void @@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[]) { int fd; - fd = open(GYRO_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no gyro found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ - ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if (argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { @@ -206,41 +206,41 @@ do_mag(int argc, char *argv[]) { int fd; - fd = open(MAG_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no magnetometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ - ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -282,41 +282,41 @@ do_accel(int argc, char *argv[]) { int fd; - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no accelerometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e8..4e27105723 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1600 +MODULE_STACKSIZE = 1500 MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index ceaea35b63..50547a5626 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; + /* back off 800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 800U * 1000U) { + usleep(50000); + } + /* try to bring up the console - stop doing so if the system gets armed */ while (true) { diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 2ead3e6323..087eb52e3d 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -54,6 +54,7 @@ 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 const char mcu_uid_str[] = "uid"; static void usage(const char *reason) { @@ -61,7 +62,7 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[]) ret = 0; } + if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) { + uint32_t uid[3]; + + mcu_unique_id(uid); + + printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + + ret = 0; + } + + if (ret == 1) { errx(1, "unknown command.\n"); }