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

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

This commit is contained in:
Thomas Gubler 2015-01-28 16:29:14 +01:00
parent f23e603d02 1dcc5c49cc
commit 2728889f78
45 changed files with 938 additions and 347 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
.travis.yml
View File

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

88
Debug/Nuttx.py
View File

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

260
Debug/poor-mans-profiler.sh Executable file
View File

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

BIN
Documentation/px4_block_diagram.odg Normal file
View File

Binary file not shown.

10
Makefile
View File

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

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

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

36
Tools/make_color.sh Executable file
View File

@ -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='<your-firmware-directory>/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/"

1
makefiles/config_px4fmu-v1_default.mk
View File

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

10
makefiles/config_px4fmu-v2_default.mk
View File

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

1
makefiles/setup.mk
View File

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

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

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

5
nuttx-configs/aerocore/nsh/Make.defs
View File

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

2
nuttx-configs/aerocore/nsh/defconfig
View File

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

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

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

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

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

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

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

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

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

24
src/drivers/batt_smbus/batt_smbus.cpp
View File

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

3
src/drivers/drv_range_finder.h
View File

@ -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 */
};
/**

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

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

189
src/drivers/mb12xx/mb12xx.cpp
View File

@ -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 <jon.verbeke@kuleuven.be>
*
* Driver for the Maxbotix sonar range finders connected via I2C.
*/
@ -54,6 +55,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <vector>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
@ -72,7 +74,7 @@
#include <board_config.h>
/* 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<uint8_t> addr_ind; /* temp sonar i2c address vector */
std::vector<float> _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<struct range_finder_report *>(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[])

42
src/drivers/mpu6000/mpu6000.cpp
View File

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

82
src/drivers/px4flow/i2c_frame.h Normal file
View File

@ -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 <thomas.boehm@fortiss.org>
* @author James Goppert <james.goppert@gmail.com>
*/
#ifndef I2C_FRAME_H_
#define I2C_FRAME_H_
#include <inttypes.h>
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_ */

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

@ -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<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
report.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);

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

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

1
src/drivers/px4io/px4io_uploader.cpp
View File

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

2
src/lib/uavcan

@ -1 +1 @@
Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d
Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68

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

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

2
src/modules/mavlink/mavlink_receiver.cpp
View File

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

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -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 <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>

15
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

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

116
src/modules/navigator/geofence.cpp
View File

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

3
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

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

12
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

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

2
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

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

11
src/modules/systemlib/mcu_version.c
View File

@ -47,7 +47,8 @@
#ifdef CONFIG_ARCH_CHIP_STM32
#include <up_arch.h>
#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)
{

11
src/modules/systemlib/mcu_version.h
View File

@ -33,6 +33,8 @@
#pragma once
#include <stdint.h>
/* 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.
*

7
src/modules/uavcan/actuators/esc.cpp
View File

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

6
src/modules/uavcan/module.mk
View File

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

26
src/modules/uavcan/uavcan_main.cpp
View File

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

18
src/modules/uavcan/uavcan_main.hpp
View File

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

70
src/systemcmds/config/config.c
View File

@ -36,7 +36,7 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
* config tool.
* config tool. Takes the device name as the first parameter.
*/
#include <nuttx/config.h>
@ -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) {

2
src/systemcmds/nshterm/module.mk
View File

@ -38,7 +38,7 @@
MODULE_COMMAND = nshterm
SRCS = nshterm.c
MODULE_STACKSIZE = 1600
MODULE_STACKSIZE = 1500
MAXOPTIMIZATION = -Os

6
src/systemcmds/nshterm/nshterm.c
View File

@ -50,6 +50,7 @@
#include <apps/nsh.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/actuator_armed.h>
@ -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) {

14
src/systemcmds/ver/ver.c
View File

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