diff options
213 files changed, 7667 insertions, 5588 deletions
diff --git a/.gitignore b/.gitignore index d4e2a4637..611325444 100644 --- a/.gitignore +++ b/.gitignore @@ -40,6 +40,8 @@ tags .ropeproject *.orig src/modules/uORB/topics/* +src/platforms/nuttx/px4_messages/* +src/platforms/ros/px4_messages/* Firmware.zip unittests/build *.generated.h diff --git a/.travis.yml b/.travis.yml index 9386ddaa3..a994d3471 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/CMakeLists.txt b/CMakeLists.txt index 2de236ff6..0c81607b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-D__PX4_ROS) +add_definitions(-D__EXPORT=) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -68,6 +69,11 @@ add_message_files( actuator_armed.msg parameter_update.msg vehicle_status.msg + vehicle_local_position.msg + position_setpoint.msg + position_setpoint_triplet.msg + vehicle_local_position_setpoint.msg + vehicle_global_velocity_setpoint.msg ) ## Generate services in the 'srv' folder @@ -116,6 +122,7 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} src/platforms + src/platforms/ros/px4_messages src/include src/modules src/ @@ -123,6 +130,16 @@ include_directories( ${EIGEN_INCLUDE_DIRS} ) +## generate multiplatform wrapper headers +## note that the message header files are generated as in any ROS project with generate_messages() +set(MULTIPLATFORM_HEADER_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src/platforms/ros/px4_messages) +set(MULTIPLATFORM_TEMPLATE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/msg/templates/px4/ros) +set(TOPICHEADER_TEMP_DIR ${CMAKE_BINARY_DIR}/topics_temporary) +set(MULTIPLATFORM_PREFIX px4_) +add_custom_target(multiplatform_message_headers ALL ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/Tools/px_generate_uorb_topic_headers.py + -d ${CMAKE_CURRENT_SOURCE_DIR}/msg -o ${MULTIPLATFORM_HEADER_DIR} -e ${MULTIPLATFORM_TEMPLATE_DIR} + -t ${TOPICHEADER_TEMP_DIR} -p ${MULTIPLATFORM_PREFIX}) + ## Declare a cpp library add_library(px4 src/platforms/ros/px4_ros_impl.cpp @@ -131,7 +148,7 @@ add_library(px4 src/lib/mathlib/math/Limits.cpp src/modules/systemlib/circuit_breaker.cpp ) -add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(px4 ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(px4 ${catkin_LIBRARIES} @@ -141,7 +158,7 @@ target_link_libraries(px4 add_executable(publisher src/examples/publisher/publisher_main.cpp src/examples/publisher/publisher_example.cpp) -add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(publisher ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(publisher ${catkin_LIBRARIES} px4 @@ -151,7 +168,7 @@ target_link_libraries(publisher add_executable(subscriber src/examples/subscriber/subscriber_main.cpp src/examples/subscriber/subscriber_example.cpp) -add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp) +add_dependencies(subscriber ${PROJECT_NAME}_generate_messages_cpp multiplatform_message_headers) target_link_libraries(subscriber ${catkin_LIBRARIES} px4 @@ -168,6 +185,16 @@ target_link_libraries(mc_att_control px4 ) +## MC Position Control +add_executable(mc_pos_control + src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp + src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp) +add_dependencies(mc_pos_control ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mc_pos_control + ${catkin_LIBRARIES} + px4 +) + ## Attitude Estimator dummy add_executable(attitude_estimator src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp) @@ -177,6 +204,15 @@ target_link_libraries(attitude_estimator px4 ) +## Position Estimator dummy +add_executable(position_estimator + src/platforms/ros/nodes/position_estimator/position_estimator.cpp) +add_dependencies(position_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(position_estimator + ${catkin_LIBRARIES} + px4 +) + ## Manual input add_executable(manual_input src/platforms/ros/nodes/manual_input/manual_input.cpp) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index cf86dd668..826e12c97 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -1,7 +1,10 @@ # GDB/Python functions for dealing with NuttX +from __future__ import print_function import gdb, gdb.types +parse_int = lambda x: int(str(x), 0) + class NX_register_set(object): """Copy of the registers for a given context""" @@ -155,7 +158,7 @@ class NX_task(object): result = [] for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): entry = pidhash_value[i] - pid = int(entry['pid']) + pid = parse_int(entry['pid']) if pid is not -1: result.append(pid) return result @@ -184,7 +187,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, int(stack_limit)): + for offset in range(0, parse_int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -244,7 +247,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = int(filearray[i]['f_inode']) + inode = parse_int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -299,7 +302,7 @@ class NX_show_task (gdb.Command): super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) def invoke(self, arg, from_tty): - t = NX_task.for_pid(int(arg)) + t = NX_task.for_pid(parse_int(arg)) if t is not None: my_fmt = 'PID:{pid} name:{name} state:{state}\n' my_fmt += ' stack used {stack_used} of {stack_limit}\n' @@ -435,12 +438,12 @@ class NX_tcb(object): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -469,12 +472,12 @@ class NX_check_stack_order(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -488,7 +491,7 @@ class NX_check_stack_order(gdb.Command): def getSPfromTask(self,tcb): regmap = NX_register_set.v7em_regmap a =tcb['xcp']['regs'] - return int(a[regmap['SP']]) + return parse_int(a[regmap['SP']]) def find_closest(self,list,val): tmp_list = [abs(i-val) for i in list] @@ -525,8 +528,8 @@ class NX_check_stack_order(gdb.Command): for t in tcb: p = []; #print(t.name,t._tcb['stack_alloc_ptr']) - p.append(int(t['stack_alloc_ptr'])) - p.append(int(t['adj_stack_ptr'])) + p.append(parse_int(t['stack_alloc_ptr'])) + p.append(parse_int(t['adj_stack_ptr'])) p.append(self.getSPfromTask(t)) stackadresses[str(t['name'])] = p; address = int("0x30000000",0) @@ -594,12 +597,12 @@ class NX_search_tcb(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def invoke(self,args,sth): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -612,7 +615,7 @@ class NX_search_tcb(gdb.Command): # filter for tasks that are listed twice tasks_filt = {} for t in tasks: - pid = int(t['pid']); + pid = parse_int(t['pid']); if not pid in tasks_filt.keys(): tasks_filt[pid] = t['name']; print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) @@ -653,62 +656,49 @@ class NX_my_bt(gdb.Command): tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer()) return tcb_ptr.dereference() - def print_instruction_at(self,addr,stack_percentage): + def resolve_file_line_func(self,addr,stack_percentage): gdb.write(str(round(stack_percentage,2))+":") str_to_eval = "info line *"+hex(addr) #gdb.execute(str_to_eval) res = gdb.execute(str_to_eval,to_string = True) # get information from results string: words = res.split() - valid = False - if words[0] == 'No': - #no line info... - pass - else: - valid = True + if words[0] != 'No': line = int(words[1]) - idx = words[3].rfind("/"); #find first backslash - if idx>0: - name = words[3][idx+1:]; - path = words[3][:idx]; - else: - name = words[3]; - path = ""; block = gdb.block_for_pc(addr) func = block.function if str(func) == "None": func = block.superblock.function - - if valid: - print("Line: ",line," in ",path,"/",name,"in ",func) - return name,path,line,func - - - + return words[3].strip('"'), line, func def invoke(self,args,sth): - addr_dec = int(args[2:],16) - _tcb = self.get_tcb_from_address(addr_dec) + try: + addr_dec = parse_int(args) # Trying to interpret the input as TCB address + except ValueError: + for task in NX_task.tasks(): # Interpreting as a task name + if task.name == args: + _tcb = task._tcb + break + else: + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) - up_stack = int(_tcb['adj_stack_ptr']) - curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer - other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer - stacksize = int(_tcb['adj_stack_size']) # other stack pointer + up_stack = parse_int(_tcb['adj_stack_ptr']) + curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = parse_int(_tcb['adj_stack_size']) # other stack pointer print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) - if curr_sp == up_stack: - sp = other_sp - else: - sp = curr_sp; - - while(sp < up_stack): + item = 0 + for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4): mem = self.readmem(sp) #print(hex(sp)," : ",hex(mem)) if self.is_in_bounds(mem): # this is a potential instruction ptr stack_percentage = (up_stack-sp)/stacksize - name,path,line,func = self.print_instruction_at(mem,stack_percentage) - sp = sp + 4; # jump up one word + filename,line,func = self.resolve_file_line_func(mem, stack_percentage) + print('#%-2d ' % item, '0x%08x in ' % mem, func, ' at ', filename, ':', line, sep='') + item += 1 NX_my_bt() diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh new file mode 100755 index 000000000..ab06a1b66 --- /dev/null +++ b/Debug/poor-mans-profiler.sh @@ -0,0 +1,260 @@ +#!/bin/bash +# +# Poor man's sampling profiler for NuttX. +# +# Usage: Install flamegraph.pl in your PATH, configure your .gdbinit, run the script with proper arguments and go +# have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly +# interfere with normal operation of the target, which means that you can't profile real-time tasks with it. +# +# Requirements: ARM GDB with Python support +# + +set -e +root=$(dirname $0)/.. + +function die() +{ + echo "$@" + exit 1 +} + +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} + +which flamegraph.pl > /dev/null || die "Install flamegraph.pl first" + +# +# Parsing the arguments. Read this section for usage info. +# +nsamples=0 +sleeptime=0.1 # Doctors recommend 7-8 hours a day +taskname= +elf=$root/Build/px4fmu-v2_default.build/firmware.elf +append=0 +fgfontsize=10 +fgwidth=1900 + +for i in "$@" +do + case $i in + --nsamples=*) + nsamples="${i#*=}" + ;; + --sleeptime=*) + sleeptime="${i#*=}" + ;; + --taskname=*) + taskname="${i#*=}" + ;; + --elf=*) + elf="${i#*=}" + ;; + --append) + append=1 + ;; + --fgfontsize=*) + fgfontsize="${i#*=}" + ;; + --fgwidth=*) + fgwidth="${i#*=}" + ;; + *) + usage + ;; + esac + shift +done + +# +# Temporary files +# +stacksfile=/tmp/pmpn-stacks.log +foldfile=/tmp/pmpn-folded.txt +graphfile=/tmp/pmpn-flamegraph.svg +gdberrfile=/tmp/pmpn-gdberr.log + +# +# Sampling if requested. Note that if $append is true, the stack file will not be rewritten. +# +cd $root + +if [[ $nsamples > 0 ]] +then + [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") + + echo "Sampling the task '$taskname'..." + + for x in $(seq 1 $nsamples) + do + if [[ "$taskname" = "" ]] + then + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" -ex bt \ + 2> $gdberrfile \ + | sed -n 's/\(#.*\)/\1/p' \ + >> $stacksfile + else + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + fi + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime + done + + echo + echo "Stacks saved to $stacksfile" +else + echo "Sampling skipped - set 'nsamples' to re-sample." +fi + +# +# Folding the stacks. +# +[ -f $stacksfile ] || die "Where are the stack samples?" + +cat << 'EOF' > /tmp/pmpn-folder.py +# +# This stack folder correctly handles C++ types. +# +from __future__ import print_function, division +import fileinput, collections, os, sys + +def enforce(x, msg='Invalid input'): + if not x: + raise Exception(msg) + +def split_first_part_with_parens(line): + LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} + RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + QUOTES = set(['"', "'"]) + quotes = collections.defaultdict(bool) + braces = collections.defaultdict(int) + out = '' + for ch in line: + out += ch + # escape character cancels further processing + if ch == '\\': + continue + # special cases + if out.endswith('operator>') or out.endswith('operator>>') or out.endswith('operator->'): # gotta love c++ + braces['<>'] += 1 + if out.endswith('operator<') or out.endswith('operator<<'): + braces['<>'] -= 1 + # switching quotes + if ch in QUOTES: + quotes[ch] = not quotes[ch] + # counting parens only when outside quotes + if sum(quotes.values()) == 0: + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 + # sanity check + for v in braces.values(): + enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) + # termination condition + if ch == ' ' and sum(braces.values()) == 0: + break + out = out.strip() + return out, line[len(out):] + +def parse(line): + def take_path(line, output): + line = line.strip() + if line.startswith('at '): + line = line[3:].strip() + if line: + output['file_full_path'] = line.rsplit(':', 1)[0].strip() + output['file_base_name'] = os.path.basename(output['file_full_path']) + output['line'] = int(line.rsplit(':', 1)[1]) + return output + + def take_args(line, output): + line = line.lstrip() + if line[0] == '(': + output['args'], line = split_first_part_with_parens(line) + return take_path(line.lstrip(), output) + + def take_function(line, output): + output['function'], line = split_first_part_with_parens(line.lstrip()) + return take_args(line.lstrip(), output) + + def take_mem_loc(line, output): + line = line.lstrip() + if line.startswith('0x'): + end = line.find(' ') + num = line[:end] + output['memloc'] = int(num, 16) + line = line[end:].lstrip() + end = line.find(' ') + enforce(line[:end] == 'in') + line = line[end:].lstrip() + return take_function(line, output) + + def take_frame_num(line, output): + line = line.lstrip() + enforce(line[0] == '#') + end = line.find(' ') + num = line[1:end] + output['frame_num'] = int(num) + return take_mem_loc(line[end:], output) + + return take_frame_num(line, {}) + +stacks = collections.defaultdict(int) +current = '' + +stack_tops = collections.defaultdict(int) +num_stack_frames = 0 + +for idx,line in enumerate(fileinput.input()): + try: + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + + if inf['frame_num'] == 0: + num_stack_frames += 1 + stack_tops[fun] += 1 + elif current: + stacks[current] += 1 + current = '' + except Exception, ex: + print('ERROR (line %d):' % (idx + 1), ex, file=sys.stderr) + +for s, f in sorted(stacks.items(), key=lambda (s, f): s): + print(s, f) + +print('Total stack frames:', num_stack_frames, file=sys.stderr) +print('Top consumers (distribution of the stack tops):', file=sys.stderr) +for name,num in sorted(stack_tops.items(), key=lambda (name, num): num, reverse=True)[:10]: + print('% 5.1f%% ' % (100 * num / num_stack_frames), name, file=sys.stderr) +EOF + +cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile + +echo "Folded stacks saved to $foldfile" + +# +# Graphing. +# +cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile +echo "FlameGraph saved to $graphfile" + +# On KDE, xdg-open prefers Gwenview by default, which doesn't handle interactive SVGs, so we need a browser. +# The current implementation is hackish and stupid. Somebody, please do something about it. +opener=xdg-open +which firefox > /dev/null && opener=firefox +which google-chrome > /dev/null && opener=google-chrome + +$opener $graphfile diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg Binary files differnew file mode 100644 index 000000000..845668442 --- /dev/null +++ b/Documentation/px4_block_diagram.odg @@ -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 @@ -225,20 +227,27 @@ updatesubmodules: $(Q) (git submodule update) MSG_DIR = $(PX4_BASE)msg -MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates +UORB_TEMPLATE_DIR = $(PX4_BASE)msg/templates/uorb +MULTIPLATFORM_TEMPLATE_DIR = $(PX4_BASE)msg/templates/px4/uorb TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics -TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary -GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src -GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src +MULTIPLATFORM_HEADER_DIR = $(PX4_BASE)src/platforms/nuttx/px4_messages +MULTIPLATFORM_PREFIX = px4_ +TOPICHEADER_TEMP_DIR = $(BUILD_DIR)topics_temporary +GENMSG_PYTHONPATH = $(PX4_BASE)Tools/genmsg/src +GENCPP_PYTHONPATH = $(PX4_BASE)Tools/gencpp/src .PHONY: generateuorbtopicheaders generateuorbtopicheaders: @$(ECHO) "Generating uORB topic headers" $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ - -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) + -d $(MSG_DIR) -o $(TOPICS_DIR) -e $(UORB_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR)) + @$(ECHO) "Generating multiplatform uORB topic wrapper headers" + $(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \ + $(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \ + -d $(MSG_DIR) -o $(MULTIPLATFORM_HEADER_DIR) -e $(MULTIPLATFORM_TEMPLATE_DIR) -t $(TOPICHEADER_TEMP_DIR) -p $(MULTIPLATFORM_PREFIX)) # clean up temporary files - $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) + $(Q) (rm -r $(TOPICHEADER_TEMP_DIR)) # # Testing targets diff --git a/NuttX b/NuttX -Subproject 3d8171f6ea88297d8595525c8222d61e9cf20fd +Subproject 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb970 @@ -1,6 +1,6 @@ ## PX4 Flight Control Stack and Middleware ## -[](https://travis-ci.org/PX4/Firmware) +[](https://travis-ci.org/PX4/Firmware) [](https://scan.coverity.com/projects/3966?tab=overview) [](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) @@ -21,7 +21,8 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl ### Developers ### Contributing guide: -http://px4.io/dev/contributing + * [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md) + * [PX4 Contribution Guide](http://px4.io/dev/contributing) Developer guide: http://px4.io/dev/ diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9aca3fc5f..9e2d1f6ff 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,12 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# +land_detector start fixedwing + +# +# Misc apps +# diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 25f31a7e0..a14eb5247 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 4 -t + sdlog2 start -r 40 -a -b 3 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 22 -t diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 2248b0bcd..2ecc104df 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,4 +14,15 @@ else # try the multiplatform version mc_att_control_m start fi -mc_pos_control start + +if mc_pos_control start +then +else + # try the multiplatform version + mc_pos_control_m start +fi + +# +# Start Land Detector +# +land_detector start multicopter diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff0..4d337171a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -6,6 +6,11 @@ # # +# Start CDC/ACM serial driver +# +sercon + +# # Default to auto-start mode. # set MODE autostart @@ -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/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 40b3500e0..4b9a9b68a 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -97,6 +97,13 @@ else set unit_test_failure_list "${unit_test_failure_list} commander_tests" fi +if uorb test +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} uorb_tests" +fi + if hmc5883 -I start then # This is an FMUv3 diff --git a/Tools/make_color.sh b/Tools/make_color.sh new file mode 100755 index 000000000..81316a932 --- /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='<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/" diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 2ddbd6984..4bcab4d54 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -60,7 +60,7 @@ def convert_file(filename, outputdir, templatedir, includepath): """ Converts a single .msg file to a uorb header """ - print("Generating uORB headers from {0}".format(filename)) + print("Generating headers from {0}".format(filename)) genmsg.template_tools.generate_from_file(filename, package, outputdir, @@ -85,16 +85,21 @@ def convert_dir(inputdir, outputdir, templatedir): includepath) -def copy_changed(inputdir, outputdir): +def copy_changed(inputdir, outputdir, prefix=''): """ Copies files from inputdir to outputdir if they don't exist in ouputdir or if their content changed """ + + # Make sure output directory exists: + if not os.path.isdir(outputdir): + os.makedirs(outputdir) + for f in os.listdir(inputdir): fni = os.path.join(inputdir, f) if os.path.isfile(fni): # Check if f exists in outpoutdir, copy the file if not - fno = os.path.join(outputdir, f) + fno = os.path.join(outputdir, prefix + f) if not os.path.isfile(fno): shutil.copy(fni, fno) print("{0}: new header file".format(f)) @@ -108,7 +113,8 @@ def copy_changed(inputdir, outputdir): print("{0}: unchanged".format(f)) -def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir, prefix): """ Converts all .msg files in inputdir to uORB header files Unchanged existing files are not overwritten. @@ -117,7 +123,7 @@ def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): convert_dir(inputdir, temporarydir, templatedir) # Copy changed headers from temporary dir to output dir - copy_changed(temporarydir, outputdir) + copy_changed(temporarydir, outputdir, prefix) if __name__ == "__main__": parser = argparse.ArgumentParser( @@ -132,6 +138,9 @@ if __name__ == "__main__": help='output directory for header files') parser.add_argument('-t', dest='temporarydir', help='temporary directory') + parser.add_argument('-p', dest='prefix', default='', + help='string added as prefix to the output file ' + ' name when converting directories') args = parser.parse_args() if args.file is not None: @@ -146,4 +155,5 @@ if __name__ == "__main__": args.dir, args.outputdir, args.templatedir, - args.temporarydir) + args.temporarydir, + args.prefix) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c46f6bede..95a3d4046 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -266,18 +266,19 @@ class uploader(object): self.__getSync() return value - def __drawProgressBar(self, progress, maxVal): + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal percent = (float(progress) / float(maxVal)) * 100.0 - sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent)) + sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent)) sys.stdout.flush() # send the CHIP_ERASE command and wait for the bootloader to become ready - def __erase(self): + def __erase(self, label): + print("\n", end='') self.__send(uploader.CHIP_ERASE + uploader.EOC) @@ -288,15 +289,14 @@ class uploader(object): #Draw progress bar (erase usually takes about 9 seconds to complete) estimatedTimeRemaining = deadline-time.time() if estimatedTimeRemaining >= 9.0: - self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) + self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0) else: - self.__drawProgressBar(10.0, 10.0) + self.__drawProgressBar(label, 10.0, 10.0) sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) sys.stdout.flush() if self.__trySync(): - self.__drawProgressBar(10.0, 10.0) - sys.stdout.write("\nerase complete!\n") + self.__drawProgressBar(label, 10.0, 10.0) return; raise RuntimeError("timed out waiting for erase") @@ -350,7 +350,8 @@ class uploader(object): return [seq[i:i+length] for i in range(0, len(seq), length)] # upload code - def __program(self, fw): + def __program(self, label, fw): + print("\n", end='') code = fw.image groups = self.__split_len(code, uploader.PROG_MULTI_MAX) @@ -361,31 +362,40 @@ class uploader(object): #Print upload progress (throttled, so it does not delay upload progress) uploadProgress += 1 if uploadProgress % 256 == 0: - self.__drawProgressBar(uploadProgress, len(groups)) - self.__drawProgressBar(100, 100) - print("\nprogram complete!") + self.__drawProgressBar(label, uploadProgress, len(groups)) + self.__drawProgressBar(label, 100, 100) # verify code - def __verify_v2(self, fw): + def __verify_v2(self, label, fw): + print("\n", end='') self.__send(uploader.CHIP_VERIFY + uploader.EOC) self.__getSync() code = fw.image groups = self.__split_len(code, uploader.READ_MULTI_MAX) + verifyProgress = 0 for bytes in groups: + verifyProgress += 1 + if verifyProgress % 256 == 0: + self.__drawProgressBar(label, verifyProgress, len(groups)) if (not self.__verify_multi(bytes)): raise RuntimeError("Verification failed") + self.__drawProgressBar(label, 100, 100) - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) + def __verify_v3(self, label, fw): + print("\n", end='') + self.__drawProgressBar(label, 1, 100) + expect_crc = fw.crc(self.fw_maxsize) self.__send(uploader.GET_CRC + uploader.EOC) report_crc = self.__recv_int() self.__getSync() + verifyProgress = 0 if report_crc != expect_crc: print("Expected 0x%x" % expect_crc) print("Got 0x%x" % report_crc) raise RuntimeError("Program CRC failed") + self.__drawProgressBar(label, 100, 100) # get basic data about the board def identify(self): @@ -439,19 +449,16 @@ class uploader(object): except Exception: # ignore bad character encodings pass - print("erase...") - self.__erase() - - print("program...") - self.__program(fw) + + self.__erase("Erase ") + self.__program("Program", fw) - print("verify...") if self.bl_rev == 2: - self.__verify_v2(fw) + self.__verify_v2("Verify ", fw) else: - self.__verify_v3(fw) + self.__verify_v3("Verify ", fw) - print("done, rebooting.") + print("\nRebooting.\n") self.__reboot() self.port.close() diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 96ff3ad99..0f8cc5132 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -6,7 +6,9 @@ <node pkg="px4" name="commander" type="commander"/> <node pkg="px4" name="mc_mixer" type="mc_mixer"/> <node pkg="px4" name="attitude_estimator" type="attitude_estimator"/> + <node pkg="px4" name="position_estimator" type="position_estimator"/> <node pkg="px4" name="mc_att_control" type="mc_att_control"/> + <node pkg="px4" name="mc_pos_control" type="mc_pos_control"/> </group> </launch> diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 7eb090d73..4e528f8cd 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 2ba07821f..2ce738844 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,6 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index bed2fa506..29eb68096 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -87,7 +87,10 @@ MODULES += modules/position_estimator_inav #MODULES += modules/fw_att_control # MODULES += modules/mc_att_control MODULES += modules/mc_att_control_multiplatform -MODULES += modules/mc_pos_control +MODULES += examples/subscriber +MODULES += examples/publisher +# MODULES += modules/mc_pos_control +MODULES += modules/mc_pos_control_multiplatform MODULES += modules/vtol_att_control # diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index fa2c83262..af80cae8f 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +MODULES += systemcmds/top # # System commands diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 435c240bf..351b9f1b7 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 396980453..dc8d3f712 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/msg/actuator_controls_1.msg b/msg/actuator_controls_1.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_1.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_2.msg b/msg/actuator_controls_2.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_2.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_3.msg b/msg/actuator_controls_3.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_3.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/actuator_controls_virtual_fw.msg b/msg/actuator_controls_virtual_fw.msg new file mode 100644 index 000000000..414eb06dd --- /dev/null +++ b/msg/actuator_controls_virtual_fw.msg @@ -0,0 +1,5 @@ +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 +uint64 timestamp +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control diff --git a/msg/fw_virtual_rates_setpoint.msg b/msg/fw_virtual_rates_setpoint.msg new file mode 100644 index 000000000..834113c79 --- /dev/null +++ b/msg/fw_virtual_rates_setpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # in microseconds since system start + +float32 roll # body angular rates in NED frame +float32 pitch # body angular rates in NED frame +float32 yaw # body angular rates in NED frame +float32 thrust # thrust normalized to 0..1 diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg new file mode 100644 index 000000000..ae6756aa8 --- /dev/null +++ b/msg/position_setpoint.msg @@ -0,0 +1,35 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) +uint8 SETPOINT_TYPE_OFFBOARD=6 # setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller +float32 x # local position setpoint in m in NED +float32 y # local position setpoint in m in NED +float32 z # local position setpoint in m in NED +bool position_valid # true if local position setpoint valid +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED +bool velocity_valid # true if local velocity setpoint valid +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw +bool yaw_valid # true if yaw setpoint valid +float32 yawspeed # yawspeed (only for multirotors, in rad/s) +bool yawspeed_valid # true if yawspeed setpoint valid +float32 loiter_radius # loiter radius (only for fixed wing), in m +int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW +float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints +float32 a_x # acceleration x setpoint +float32 a_y # acceleration y setpoint +float32 a_z # acceleration z setpoint +bool acceleration_valid # true if acceleration setpoint is valid/should be used +bool acceleration_is_force # interprete acceleration as force diff --git a/msg/position_setpoint_triplet.msg b/msg/position_setpoint_triplet.msg new file mode 100644 index 000000000..8717f65d0 --- /dev/null +++ b/msg/position_setpoint_triplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +px4/position_setpoint previous +px4/position_setpoint current +px4/position_setpoint next + +uint8 nav_state # report the navigation state diff --git a/msg/templates/px4/ros/msg.h.template b/msg/templates/px4/ros/msg.h.template new file mode 100644 index 000000000..76abab163 --- /dev/null +++ b/msg/templates/px4/ros/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include "px4/@(topic_name).h" +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return "@(topic_name)";} +}; + +}; diff --git a/msg/templates/px4/uorb/msg.h.template b/msg/templates/px4/uorb/msg.h.template new file mode 100644 index 000000000..2d4251107 --- /dev/null +++ b/msg/templates/px4/uorb/msg.h.template @@ -0,0 +1,95 @@ +@############################################### +@# +@# PX4 ROS compatible message source code +@# generation for C++ +@# +@# This file generates the multiplatform wrapper +@# +@# EmPy template for generating <msg>.h files +@# Based on the original template for ROS +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - file_name_in (String) Source file +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - md5sum (String) MD5Sum of the .msg specification +@############################################### +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* Auto-generated by genmsg_cpp from file @file_name_in */ + +@{ +import genmsg.msgs +import gencpp + +cpp_class = 'px4_%s'%spec.short_name +native_type = '%s_s'%spec.short_name +topic_name = spec.short_name +}@ + +#pragma once + +@############################## +@# Generic Includes +@############################## +#include <uORB/topics/@(topic_name).h> +#include "platforms/px4_message.h" + +@############################## +@# Class +@############################## + +namespace px4 +{ + +class __EXPORT @(cpp_class) : + public PX4Message<@(native_type)> +{ +public: + @(cpp_class)() : + PX4Message<@(native_type)>() + {} + + @(cpp_class)(@(native_type) msg) : + PX4Message<@(native_type)>(msg) + {} + + ~@(cpp_class)() {} + + static PX4TopicHandle handle() {return ORB_ID(@(topic_name));} +}; + +}; diff --git a/msg/templates/msg.h.template b/msg/templates/uorb/msg.h.template index 19068a3a1..f16095c97 100644 --- a/msg/templates/msg.h.template +++ b/msg/templates/uorb/msg.h.template @@ -16,7 +16,7 @@ @############################################### /**************************************************************************** * - * Copyright (C) 2013-2014 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 @@ -47,17 +47,14 @@ * ****************************************************************************/ - /* Auto-generated by genmsg_cpp from file @file_name_in */ +/* Auto-generated by genmsg_cpp from file @file_name_in */ @{ import genmsg.msgs import gencpp -cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace -cpp_class = '%s_'%spec.short_name -cpp_full_name = '%s%s'%(cpp_namespace,cpp_class) -cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name) -cpp_msg_definition = gencpp.escape_message_definition(msg_definition) +uorb_struct = '%s_s'%spec.short_name +topic_name = spec.short_name }@ #pragma once @@ -66,7 +63,7 @@ cpp_msg_definition = gencpp.escape_message_definition(msg_definition) @# Generic Includes @############################## #include <stdint.h> -#include "../uORB.h" +#include <uORB/uORB.h> @############################## @# Includes for dependencies @@ -80,10 +77,13 @@ for field in spec.parsed_fields(): print('#include <uORB/topics/%s.h>'%(name)) }@ -@# Constants + +@# Constants c style +#ifndef __cplusplus @[for constant in spec.constants]@ #define @(constant.name) @(int(constant.val)) @[end for] +#endif /** * @@addtogroup topics @@ -104,8 +104,10 @@ type_map = {'int8': 'int8_t', 'uint32': 'uint32_t', 'uint64': 'uint64_t', 'float32': 'float', + 'float64': 'double', 'bool': 'bool', - 'fence_vertex': 'fence_vertex'} + 'fence_vertex': 'fence_vertex', + 'position_setpoint': 'position_setpoint'} # Function to print a standard ros type def print_field_def(field): @@ -136,13 +138,33 @@ def print_field_def(field): print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size)) } -struct @(spec.short_name)_s { +#ifdef __cplusplus +@#class @(uorb_struct) { +struct __EXPORT @(uorb_struct) { +@#public: +#else +struct @(uorb_struct) { +#endif @{ # loop over all fields and print the type and name for field in spec.parsed_fields(): if (not field.is_header): print_field_def(field) }@ +#ifdef __cplusplus +@# Constants again c++-ified +@{ +for constant in spec.constants: + type = constant.type + if type in type_map: + # need to add _t: int8 --> int8_t + type_px4 = type_map[type] + else: + raise Exception("Type {0} not supported, add to to template file!".format(type)) + + print('\tstatic const %s %s = %s;'%(type_px4, constant.name, int(constant.val))) +} +#endif }; /** @@ -150,4 +172,4 @@ for field in spec.parsed_fields(): */ /* register this as object request broker structure */ -ORB_DECLARE(@(spec.short_name)); +ORB_DECLARE(@(topic_name)); diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index dbfd8b6bc..98018a1df 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -16,20 +16,3 @@ float32[4] q # Quaternion (NED) float32[3] g_comp # Compensated gravity vector bool R_valid # Rotation matrix valid bool q_valid # Quaternion valid - -# secondary attitude for VTOL -float32 roll_sec # Roll angle (rad, Tait-Bryan, NED) -float32 pitch_sec # Pitch angle (rad, Tait-Bryan, NED) -float32 yaw_sec # Yaw angle (rad, Tait-Bryan, NED) -float32 rollspeed_sec # Roll angular speed (rad/s, Tait-Bryan, NED) -float32 pitchspeed_sec # Pitch angular speed (rad/s, Tait-Bryan, NED) -float32 yawspeed_sec # Yaw angular speed (rad/s, Tait-Bryan, NED) -float32 rollacc_sec # Roll angular accelration (rad/s, Tait-Bryan, NED) -float32 pitchacc_sec # Pitch angular acceleration (rad/s, Tait-Bryan, NED) -float32 yawacc_sec # Yaw angular acceleration (rad/s, Tait-Bryan, NED) -float32[3] rate_offsets_sec # Offsets of the body angular rates from zero -float32[9] R_sec # Rotation matrix, body to world, (Tait-Bryan, NED) -float32[4] q_sec # Quaternion (NED) -float32[3] g_comp_sec # Compensated gravity vector -bool R_valid_sec # Rotation matrix valid -bool q_valid_sec # Quaternion valid diff --git a/msg/vehicle_global_velocity_setpoint.msg b/msg/vehicle_global_velocity_setpoint.msg new file mode 100644 index 000000000..ca7dcc826 --- /dev/null +++ b/msg/vehicle_global_velocity_setpoint.msg @@ -0,0 +1,4 @@ +# Velocity setpoint in NED frame +float32 vx # in m/s NED +float32 vy # in m/s NED +float32 vz # in m/s NED diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg new file mode 100644 index 000000000..4da027ae7 --- /dev/null +++ b/msg/vehicle_local_position.msg @@ -0,0 +1,36 @@ +# Fused local position in NED. + +uint64 timestamp # Time of this estimate, in microseconds since system start +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # X position in meters in NED earth-fixed frame +float32 y # X position in meters in NED earth-fixed frame +float32 z # Z position in meters in NED earth-fixed frame (negative altitude) + +# Velocity in NED frame +float32 vx # Ground X Speed (Latitude), m/s in NED +float32 vy # Ground Y Speed (Longitude), m/s in NED +float32 vz # Ground Z Speed (Altitude), m/s in NED + +# Heading +float32 yaw + +# Reference position in GPS / WGS84 frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set +float64 ref_lat # Reference point latitude in degrees +float64 ref_lon # Reference point longitude in degrees +float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + +# Distance to surface +float32 dist_bottom # Distance to bottom surface (ground) +float32 dist_bottom_rate # Distance to bottom surface (ground) change rate +uint64 surface_bottom_timestamp # Time when new bottom surface found +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph +float32 epv diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg new file mode 100644 index 000000000..a2ff8a4ae --- /dev/null +++ b/msg/vehicle_local_position_setpoint.msg @@ -0,0 +1,7 @@ +# Local position in NED frame + +uint64 timestamp # timestamp of the setpoint +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED +float32 yaw # in radians NED -PI..+PI diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs index c1f5a8ac4..3808fc1cf 100644 --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 317194f68..c44b074f3 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -94,7 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 5e28f2473..4e08d28a2 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index fade1f0c6..539634e3d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140..5a1d5af2c 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ac3dbe5da..dedebdfa0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 @@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index b4f5577ae..74f38c0cb 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 7c76be7ec..385b8d6a8 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 51420eb23..287466db6 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 02b51e3d7..a9b847794 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y - +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): # diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 7f1b21a95..7d4b7d880 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,10 @@ #include <uORB/uORB.h> #include <uORB/topics/safety.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_control_mode.h> @@ -98,7 +102,7 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ @@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - + /* for now only spin if armed and immediately shut down * if in failsafe */ diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 4fa24275f..0f45b6536 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[3] = motor4; static orb_advert_t pub = 0; if (pub == 0) { - pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + /* advertise to channel 0 / primary */ + pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); } if (hrt_absolute_time() - last_motor_time > min_motor_interval) { @@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor fsync(ardrone_fd); /* publish just written values */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), pub, &outputs); if (ret == sizeof(buf)) { return OK; diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2b5fef4d7..92b752a28 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/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 6b14f5945..d0253a8d1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -571,7 +571,7 @@ BlinkM::led() printf("<blinkm> cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -595,7 +595,7 @@ BlinkM::led() led_color_8 = LED_BLUE; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + } else if(vehicle_status_raw.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -647,14 +647,14 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) + if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; /* TODO: add other Auto modes */ - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; - else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + else if (vehicle_status_raw.main_state == vehicle_status_s::MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966b..1eca6155b 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +ORB_DECLARE(sensor_accel); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 3e28d3d3d..3d275d619 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,9 +63,7 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro0); -ORB_DECLARE(sensor_baro1); -ORB_DECLARE(sensor_baro2); +ORB_DECLARE(sensor_baro); /* * ioctl() definitions diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 9c278f6c5..b757da545 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 41b013a44..5e0334a18 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro0); -ORB_DECLARE(sensor_gyro1); -ORB_DECLARE(sensor_gyro2); +ORB_DECLARE(sensor_gyro); /* * ioctl() definitions diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 5ddf5d08e..193c816e0 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,6 +44,7 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" + #define MAG_DEVICE_PATH "/dev/mag" /** @@ -79,15 +80,8 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag0); -ORB_DECLARE(sensor_mag1); -ORB_DECLARE(sensor_mag2); +ORB_DECLARE(sensor_mag); -/* - * mag device types, for _device_id - */ -#define DRV_MAG_DEVTYPE_HMC5883 1 -#define DRV_MAG_DEVTYPE_LSM303D 2 /* * ioctl() definitions diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 713618545..e0b16fa5c 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,9 @@ #define _DRV_UORB_H /** - * @file uORB published object driver. + * @file drv_orb_dev.h + * + * uORB published object driver. */ #include <sys/types.h> @@ -84,4 +86,7 @@ /** Get the global advertiser handle for the topic */ #define ORBIOCGADVERTISER _ORBIOC(13) +/** Get the priority for the topic */ +#define ORBIOCGPRIORITY _ORBIOC(14) + #endif /* _DRV_UORB_H */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f58..12d51aeaa 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/drv_sensor.h b/src/drivers/drv_sensor.h index 5e4598de8..467dace08 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -43,6 +43,24 @@ #include <stdint.h> #include <sys/ioctl.h> +#include "drv_device.h" + +/** + * Sensor type definitions. + * + * Used to create a unique device id for redundant and combo sensors + */ + +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 + /* * ioctl() definitions * diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 05dfc99b7..8157bc7e6 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); - default: warnx("ERR: baudrate: %d\n", baud); return -EINVAL; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b..767ff5803 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,8 @@ #include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_outputs.h> @@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[]) int HIL::set_mode(Mode mode) { - /* + /* * Configure for PWM output. * * Note that regardless of the configured mode, the task is always @@ -269,19 +271,19 @@ HIL::set_mode(Mode mode) /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_8PWM: debug("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_12PWM: debug("MODE_12PWM"); /* multi-port as 12 PWM outs */ _update_rate = 50; /* default output rate */ break; - + case MODE_16PWM: debug("MODE_16PWM"); /* multi-port as 16 PWM outs */ @@ -330,8 +332,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); pollfd fds[2]; fds[0].fd = _t_actuators; @@ -423,7 +426,7 @@ HIL::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); } } @@ -513,12 +516,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: - // HIL always outputs at the alternate (usually faster) rate + // HIL always outputs at the alternate (usually faster) rate break; case PWM_SERVO_SET(2): @@ -659,7 +662,7 @@ int hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; - + // /* reset to all-inputs */ // g_hil->ioctl(0, GPIO_RESET, 0); @@ -691,17 +694,17 @@ hil_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = HIL::MODE_2PWM; break; - + case PORT2_8PWM: /* select 8-pin PWM mode */ servo_mode = HIL::MODE_8PWM; break; - + case PORT2_12PWM: /* select 12-pin PWM mode */ servo_mode = HIL::MODE_12PWM; break; - + case PORT2_16PWM: /* select 16-pin PWM mode */ servo_mode = HIL::MODE_16PWM; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2a10b0063..b1605a5b0 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -66,9 +66,9 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> #include <drivers/device/ringbuffer.h> +#include <drivers/drv_device.h> #include <uORB/uORB.h> -#include <uORB/topics/subsystem_info.h> #include <float.h> #include <getopt.h> @@ -156,10 +156,9 @@ private: float _range_ga; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _mag_topic; - orb_advert_t _subsystem_pub; - orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -347,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _range_ga(1.3f), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _mag_topic(-1), - _subsystem_pub(-1), - _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -418,7 +416,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -725,6 +722,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -846,6 +846,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; + bool sensor_is_onboard = false; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); @@ -898,7 +899,8 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; - if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; @@ -921,9 +923,10 @@ HMC5883::collect() if (_mag_topic != -1) { /* publish it */ - orb_publish(_mag_orb_id, _mag_topic, &new_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(_mag_orb_id, &new_report); + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic < 0) debug("ADVERT FAIL"); @@ -1181,24 +1184,6 @@ int HMC5883::check_calibration() warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); - - - // XXX Change advertisement - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _calibrated, - SUBSYSTEM_TYPE_MAG}; - - if (!(_pub_blocked)) { - if (_subsystem_pub > 0) { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); - } else { - _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); - } - } } /* return 0 if calibrated, 1 else */ @@ -1305,9 +1290,9 @@ struct hmc5883_bus_option { uint8_t busnum; HMC5883 *dev; } bus_options[] = { - { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, #ifdef PX4_I2C_BUS_ONBOARD - { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, #endif #ifdef PX4_SPIDEV_HMC { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, @@ -1347,13 +1332,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/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 782ea62fe..f86c1af6b 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -53,6 +53,7 @@ #include <drivers/device/i2c.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "hmc5883.h" @@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus) HMC5883_I2C::HMC5883_I2C(int bus) : I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_I2C::~HMC5883_I2C() @@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: ret = -EINVAL; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 25a2f2b40..aec990ca8 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -53,6 +53,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "hmc5883.h" #include <board_config.h> @@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus) HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_SPI::~HMC5883_SPI() @@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: { ret = -EINVAL; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 08bc1fead..f583bced4 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +33,7 @@ /** * @file l3gd20.cpp - * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI. * * Note: With the exception of the self-test feature, the ST L3G4200D is * also supported by this driver. @@ -179,6 +179,12 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif @@ -214,14 +220,14 @@ private: struct hrt_call _call; unsigned _call_interval; - + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; - orb_id_t _orb_id; + int _orb_class_instance; int _class_instance; unsigned _current_rate; @@ -274,6 +280,13 @@ private: void disable_i2c(); /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), - _orb_id(nullptr), + _orb_class_instance(-1), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; + // default scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; @@ -456,20 +471,6 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _orb_id = ORB_ID(sensor_gyro2); - break; - } - reset(); measure(); @@ -478,7 +479,8 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - _gyro_topic = orb_advertise(_orb_id, &grp); + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -867,7 +869,7 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ @@ -911,7 +913,7 @@ L3GD20::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -974,7 +976,7 @@ L3GD20::measure() we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort */ - perf_count(_errors); + perf_count(_errors); return; } #endif @@ -994,7 +996,7 @@ L3GD20::measure() */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_bad_registers); - + switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: @@ -1050,7 +1052,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _gyro_topic, &report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); } _read++; @@ -1072,7 +1074,7 @@ L3GD20::print_info() for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i]); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 57754e4c0..2a0bf522b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 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 @@ -211,6 +211,12 @@ static const int ERROR = -1; #define LSM303D_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -275,7 +281,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; @@ -295,7 +301,7 @@ private: enum Rotation _rotation; - // values used to + // values used to float _last_accel[3]; uint8_t _constant_accel_count; @@ -330,6 +336,13 @@ private: void disable_i2c(); /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -507,7 +520,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; - orb_id_t _mag_orb_id; + int _mag_orb_class_instance; int _mag_class_instance; void measure(); @@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _constant_accel_count(0), _checked_next(0) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; + + /* Prime _mag with parents devid. */ + _mag->_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + + // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; @@ -618,7 +638,6 @@ LSM303D::init() if (_accel_reports == nullptr) goto out; - /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -641,26 +660,14 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - switch (_mag->_mag_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag2); - break; - } - - _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ @@ -668,21 +675,8 @@ LSM303D::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic < 0) { warnx("ADVERT ERR"); @@ -712,7 +706,7 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ @@ -746,7 +740,7 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - + if (success) { _checked_values[0] = WHO_I_AM; return OK; @@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - + case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) @@ -1424,7 +1418,7 @@ LSM303D::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1548,7 +1542,7 @@ LSM303D::measure() perf_count(_bad_values); _constant_accel_count = 0; } - + _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; @@ -1570,7 +1564,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; @@ -1641,7 +1635,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1666,7 +1660,7 @@ LSM303D::print_info() for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i]); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); @@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), - _mag_orb_id(nullptr), + _mag_orb_class_instance(-1), _mag_class_instance(-1) { } @@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) int LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) { - return _parent->mag_ioctl(filp, cmd, arg); + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, cmd, arg); + } } void diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf568658..a4fb7bcc2 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 <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[]) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb..1331744ae 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include <drivers/drv_mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/esc_status.h> @@ -456,8 +457,9 @@ MK::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_HIGH); /* advertise the blctrl status */ esc_status_s esc; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea9..e322e8b3a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -175,6 +175,12 @@ #define MPU6000_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + /* the MPU6000 can only handle high SPI bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -234,7 +240,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -361,6 +367,13 @@ private: uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** * Measurement self test * * @return 0 on success, 1 on failure @@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -457,7 +470,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; - orb_id_t _gyro_orb_id; + int _gyro_orb_class_instance; int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ @@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, @@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -596,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -613,22 +633,8 @@ MPU6000::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic < 0) { warnx("ADVERT FAIL"); @@ -639,22 +645,8 @@ MPU6000::init() struct gyro_report grp; _gyro_reports->get(&grp); - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); - break; - - } - - _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { warnx("ADVERT FAIL"); @@ -683,7 +675,7 @@ int MPU6000::reset() // for it to come out of sleep write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - + // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); @@ -741,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -921,26 +913,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) + /* + * 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.x_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.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_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.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + + /* 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% @@ -958,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -988,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | BITS_GYRO_ST_Z); // accel 8g, self-test enabled all axes @@ -1061,7 +1077,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } for (uint8_t i=0; i<3; i++) { float diff = gyro[i]-gyro_baseline[i]; float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; @@ -1076,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1223,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1512,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1631,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1692,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1713,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1739,12 +1755,12 @@ MPU6000::measure() perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1767,7 +1783,7 @@ MPU6000::print_info() for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) { uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED); if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", + ::printf("reg %02x:%02x should be %02x\n", (unsigned)_checked_registers[i], (unsigned)v, (unsigned)_checked_values[i]); @@ -1794,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), - _gyro_orb_id(nullptr), + _gyro_orb_class_instance(-1), _gyro_class_instance(-1) { } @@ -1839,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen) int MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { - return _parent->gyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc9..75b1f65fd 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +57,7 @@ #include <nuttx/clock.h> #include <arch/board/board.h> +#include <board_config.h> #include <drivers/device/device.h> #include <drivers/drv_baro.h> @@ -134,8 +135,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; - orb_id_t _orb_id; - + int _orb_class_instance; int _class_instance; perf_counter_t _sample_perf; @@ -172,6 +172,13 @@ protected: void cycle(); /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + + /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. * @@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _orb_class_instance(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), @@ -263,7 +271,6 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); - _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -303,7 +310,9 @@ MS5611::init() ret = OK; - _baro_topic = orb_advertise(_orb_id, &brp); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -725,7 +734,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _baro_topic, &report); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } if (_reports->force(&report)) { diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index f0b3fd61d..3f1f6c473 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -37,12 +37,12 @@ * Shared defines for the ms5611 driver. */ -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 000000000..b391b1f6a --- /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 <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_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f8..bb0cdbbb6 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; -}; -struct i2c_frame f; +#include "i2c_frame.h" -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_frame f; 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); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 436672040..8fcdc8023 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -70,6 +70,10 @@ #include <drivers/drv_rc_input.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> @@ -138,11 +142,11 @@ private: uint32_t _groups_required; uint32_t _groups_subscribed; - int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; - actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _actuator_output_topic; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + int _actuator_output_topic_instance; + pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; pwm_limit_t _pwm_limit; @@ -256,7 +260,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, - _actuator_output_topic(nullptr), + _actuator_output_topic_instance(-1), _poll_fds_num(0), _pwm_limit{}, _failsafe_pwm{0}, @@ -327,8 +331,6 @@ PX4FMU::init() log("default PWM output device"); } - _actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance); - /* reset GPIOs */ gpio_reset(); @@ -510,7 +512,7 @@ PX4FMU::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; _poll_fds_num = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); @@ -587,7 +589,7 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { orb_set_interval(_control_subs[i], update_rate_in_ms); } @@ -614,7 +616,7 @@ PX4FMU::task_main() /* get controls for required topics */ unsigned poll_id = 0; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[poll_id].revents & POLLIN) { orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); @@ -679,10 +681,10 @@ PX4FMU::task_main() /* publish mixed control outputs */ if (_outputs_pub < 0) { - _outputs_pub = orb_advertise(_actuator_output_topic, &outputs); + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(_actuator_output_topic, _outputs_pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } } } @@ -747,7 +749,7 @@ PX4FMU::task_main() } - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; @@ -1144,7 +1146,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) * and PWM under control of the flight config * parameters. Note that this does not allow for * changing a set of pins to be used for serial on - * FMUv1 + * FMUv1 */ switch (arg) { case 0: @@ -1637,12 +1639,15 @@ sensor_reset(int ms) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) - err(1, "servo arm failed"); + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) { + warnx("sensor rail reset failed"); + } + close(fd); } void diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 96ebedd83..ae7f85b87 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -75,6 +75,10 @@ #include <systemlib/circuit_breaker.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/safety.h> @@ -834,7 +838,7 @@ PX4IO::init() _task = task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 2000, + 1800, (main_t)&PX4IO::task_main_trampoline, nullptr); @@ -1102,7 +1106,7 @@ PX4IO::io_set_control_state(unsigned group) uint16_t regs[_max_actuators]; /* get controls */ - bool changed; + bool changed = false; switch (group) { case 0: @@ -1144,11 +1148,13 @@ PX4IO::io_set_control_state(unsigned group) break; } - if (!changed) + if (!changed) { return -1; + } - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { regs[i] = FLOAT_TO_REG(controls.control[i]); + } /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); @@ -1197,7 +1203,7 @@ PX4IO::io_set_arming_state() if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } @@ -1679,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs() /* lazily advertise on first publication */ if (_to_outputs == 0) { - _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + int instance; + _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &instance, ORB_PRIO_MAX); } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } return OK; @@ -1773,12 +1774,12 @@ PX4IO::print_debug() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 int io_fd = -1; - if (io_fd < 0) { + if (io_fd <= 0) { io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ - if (io_fd > 0) { + if (io_fd >= 0) { pollfd fds[1]; fds[0].fd = io_fd; fds[0].events = POLLIN; @@ -2278,6 +2279,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + break; case PWM_SERVO_SET_FORCE_SAFETY_OFF: /* force safety swith off */ @@ -2587,7 +2589,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_SET_RC_CONFIG: { /* enable setting of RC configuration without relying - on param_get() + on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { @@ -2853,10 +2855,10 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - printf("check CRC failed - %d\n", ret); + printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } - printf("CRCs match\n"); + printf("[PX4IO::checkcrc] CRCs match\n"); exit(0); } @@ -3003,11 +3005,14 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 2000); + if (poll(fds, 1, 2000) < 0) { + errx(1, "poll fail"); + } if (fds[0].revents == POLLIN) { - int c; - read(0, &c, 1); + /* control logic is to cancel with any key */ + char c; + (void)read(0, &c, 1); if (cancels-- == 0) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ @@ -3069,12 +3074,13 @@ lockdown(int argc, char *argv[]) if (ret > 0) { - read(0, &c, 1); + if (read(0, &c, 1) > 0) { - if (c != 'y') { - exit(0); - } else if (c == 'y') { - break; + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } } } @@ -3237,7 +3243,13 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "limit")) { if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); + int limitrate = atoi(argv[2]); + + if (limitrate > 0) { + g_dev->set_update_rate(limitrate); + } else { + errx(1, "invalid rate: %d", limitrate); + } } else { errx(1, "missing argument (50 - 500 Hz)"); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f..02e527695 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -621,6 +621,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); + up_udelay(100*1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index eb9453b50..5b945e452 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -54,6 +54,7 @@ #include <mavlink/mavlink_log.h> #include <uORB/Publication.hpp> +#include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_hrt.h> @@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor) uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); - if (nread < 6) { + if (nread < 6) { printf("failed to read\n"); return -1; } @@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; uint8_t checksum = rbuf[5]; - uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & checksum_mask; // check if checksum is valid if (checksum != checksum_computed) { @@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor) static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; - _motor1Position = float(int64_t(count) + + _motor1Position = float(int64_t(count) + _motor1Overflow*overflowAmount)/_pulsesPerRev; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; - _motor2Position = float(int64_t(count) + + _motor2Position = float(int64_t(count) + _motor2Overflow*overflowAmount)/_pulsesPerRev; } return 0; @@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) return -1; } -int RoboClaw::resetEncoders() +int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, @@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) return sum; } -int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum) { tcflush(_uart, TCIOFLUSH); // flush buffers @@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, return write(_uart, buf, n_data + 3); } -int roboclawTest(const char *deviceName, uint8_t address, +int roboclawTest(const char *deviceName, uint8_t address, uint16_t pulsesPerRev) { printf("roboclaw test: starting\n"); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index fcbb54c8e..d8b777b91 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -61,6 +61,10 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/parameter_update.h> @@ -108,7 +112,7 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { - /* + /* * The PX4 architecture provides a mixer outside of the controller. * The mixer is fed with a default vector of actuator controls, representing * moments applied to the vehicle frame. This vector * is structured as: * * Control Group 0 (attitude): - * + * * 0 - roll (-1..+1) * 1 - pitch (-1..+1) * 2 - yaw (-1..+1) @@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) * * Wikipedia description: * http://en.wikipedia.org/wiki/Publish–subscribe_pattern - * + * */ @@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* * Declare and safely initialize all structs to zero. - * + * * These structs contain the system state and things * like attitude, position, the current waypoint, etc. */ @@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (ret < 0) { /* * Poll error, this will not really happen in practice, - * but its good design practice to make output an error message. + * but its good design practice to make output an error message. */ warnx("poll error"); @@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } if (manual_sp_updated) - /* get the RC (or otherwise user based) input */ + /* get the RC (or otherwise user based) input */ orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 95ff346bb..8fd8870da 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -47,6 +47,10 @@ #include <systemlib/err.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/uORB.h> __EXPORT int ex_hwtest_main(int argc, char *argv[]); diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index a95f45d1a..145cf99cc 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0)); - int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); thread_running = true; @@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); - orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1); - orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); - orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); + orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); + orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk index 4f941cd43..62a5f8dd1 100644 --- a/src/examples/publisher/module.mk +++ b/src/examples/publisher/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = publisher SRCS = publisher_main.cpp \ + publisher_start_nuttx.cpp \ publisher_example.cpp MODULE_STACKSIZE = 1200 diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 2e5779ebe..e7ab9eb5a 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,9 +45,10 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) + _rc_channels_pub(_n.advertise<px4_rc_channels>()), + _v_att_pub(_n.advertise<px4_vehicle_attitude>()), + _parameter_update_pub(_n.advertise<px4_parameter_update>()) { - } int PublisherExample::main() @@ -55,14 +56,27 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - PX4_TOPIC_T(rc_channels) msg; - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); + loop_rate.sleep(); + _n.spinOnce(); - _rc_channels_pub->publish(msg); + /* Publish example message */ + px4_rc_channels rc_channels_msg; + rc_channels_msg.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("rc: %llu", rc_channels_msg.data().timestamp_last_valid); + _rc_channels_pub->publish(rc_channels_msg); + + /* Publish example message */ + px4_vehicle_attitude v_att_msg; + v_att_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("att: %llu", v_att_msg.data().timestamp); + _v_att_pub->publish(v_att_msg); + + /* Publish example message */ + px4_parameter_update parameter_update_msg; + parameter_update_msg.data().timestamp = px4::get_time_micros(); + PX4_INFO("param update: %llu", parameter_update_msg.data().timestamp); + _parameter_update_pub->publish(parameter_update_msg); - _n.spinOnce(); - loop_rate.sleep(); } return 0; diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47..8165b0ffc 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -37,6 +37,7 @@ * * @author Thomas Gubler <thomasgubler@gmail.com> */ +#pragma once #include <px4.h> class PublisherExample { @@ -48,5 +49,7 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher * _rc_channels_pub; + px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; + px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub; + px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub; }; diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp index 81439a8be..11439acff 100644 --- a/src/examples/publisher/publisher_main.cpp +++ b/src/examples/publisher/publisher_main.cpp @@ -37,70 +37,11 @@ * * @author Thomas Gubler <thomasgubler@gmail.com> */ -#include <string.h> -#include <cstdlib> #include "publisher_example.h" -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} -using namespace px4; - -PX4_MAIN_FUNCTION(publisher); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int publisher_main(int argc, char *argv[]); -int publisher_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: publisher {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - publisher_task_main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif +bool thread_running = false; /**< Deamon status flag */ -PX4_MAIN_FUNCTION(publisher) +int main(int argc, char **argv) { px4::init(argc, argv, "publisher"); diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp new file mode 100644 index 000000000..db9d85269 --- /dev/null +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file publisher_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int publisher_main(int argc, char *argv[]); +int publisher_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: publisher {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("publisher", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 3156ebb30..0e02c65b4 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = subscriber SRCS = subscriber_main.cpp \ + subscriber_start_nuttx.cpp \ subscriber_example.cpp \ subscriber_params.c diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 3c80561ca..e1292f788 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,8 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +void rc_channels_callback_function(const px4_rc_channels &msg) { + PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -62,11 +62,19 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - /* Class Method */ - PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + _n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval); + /* No callback */ - _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + _sub_rc_chan = _n.subscribe<px4_rc_channels>(500); + + /* Class method */ + _n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000); + + /* Another class method */ + _n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000); + + /* Yet antoher class method */ + _n.subscribe<px4_parameter_update>(&SubscriberExample::parameter_update_callback, this, 1000); PX4_INFO("subscribed"); } @@ -75,8 +83,23 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - _sub_rc_chan->get().timestamp_last_valid); +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { + PX4_INFO("rc_channels_callback (method): [%llu]", + msg.data().timestamp_last_valid); + PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", + _sub_rc_chan->data().timestamp_last_valid); +} + +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { + PX4_INFO("vehicle_attitude_callback (method): [%llu]", + msg.data().timestamp); +} + +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { + PX4_INFO("parameter_update_callback (method): [%llu]", + msg.data().timestamp); + PX4_PARAM_GET(_p_sub_interv, &_interval); + PX4_INFO("Param SUB_INTERV = %d", _interval); + PX4_PARAM_GET(_p_test_float, &_test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index eb2c956e0..9b6d890e3 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -41,7 +41,7 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg); +void rc_channels_callback_function(const px4_rc_channels &msg); class SubscriberExample { public: @@ -56,9 +56,10 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; - - void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + px4::Subscriber<px4_rc_channels> * _sub_rc_chan; + void rc_channels_callback(const px4_rc_channels &msg); + void vehicle_attitude_callback(const px4_vehicle_attitude &msg); + void parameter_update_callback(const px4_parameter_update &msg); }; diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp index 906921e01..798c74163 100644 --- a/src/examples/subscriber/subscriber_main.cpp +++ b/src/examples/subscriber/subscriber_main.cpp @@ -37,70 +37,10 @@ * * @author Thomas Gubler <thomasgubler@gmail.com> */ -#include <string.h> -#include <cstdlib> #include "subscriber_example.h" +bool thread_running = false; /**< Deamon status flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} -using namespace px4; - -PX4_MAIN_FUNCTION(subscriber); - -#if !defined(__PX4_ROS) -extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); -int subscriber_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: subscriber {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - subscriber_task_main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif - -PX4_MAIN_FUNCTION(subscriber) +int main(int argc, char **argv) { px4::init(argc, argv, "subscriber"); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp new file mode 100644 index 000000000..6129b19ac --- /dev/null +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file subscriber_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int subscriber_main(int argc, char *argv[]); +int subscriber_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: subscriber {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("subscriber", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 6f640c9f9..c709a2834 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample) // no filtering return sample; } + // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { @@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample) } float LowPassFilter2p::reset(float sample) { - _delay_element_1 = _delay_element_2 = sample; + float dval = sample / (_b0 + _b1 + _b2); + _delay_element_1 = dval; + _delay_element_2 = dval; return apply(sample); } diff --git a/src/lib/uavcan b/src/lib/uavcan -Subproject c4c45b995f5c8192c7a36c4293c201711ceac74 +Subproject 7719f7692aba67f01b6321773bb7be13f23d2f6 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index fd6bd0218..86f59f0e6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; - // compute secondary attitude - math::Matrix<3, 3> R_adapted; //modified rotation matrix - R_adapted = R; - - //move z to x - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); - //move x to z - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); - - //change direction of pitch (convert to right handed system) - R_adapted(0, 0) = -R_adapted(0, 0); - R_adapted(1, 0) = -R_adapted(1, 0); - R_adapted(2, 0) = -R_adapted(2, 0); - math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation - euler_angles_sec = R_adapted.to_euler(); - - att.roll_sec = euler_angles_sec(0); - att.pitch_sec = euler_angles_sec(1); - att.yaw_sec = euler_angles_sec(2); - - memcpy(&att.R_sec[0], &R_adapted.data[0][0], sizeof(att.R_sec)); - - att.rollspeed_sec = -x_aposteriori[2]; - att.pitchspeed_sec = x_aposteriori[1]; - att.yawspeed_sec = x_aposteriori[0]; - att.rollacc_sec = -x_aposteriori[5]; - att.pitchacc_sec = x_aposteriori[4]; - att.yawacc_sec = x_aposteriori[3]; - - att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); - att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); - att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1..d158d7a49 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 EXTRACXXFLAGS = -Wframe-larger-than=2400 diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index d8116ea11..b267209fe 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -57,6 +57,10 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/wind_estimate.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/vehicle_global_position.h> diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d4cd97be6..13ab966ab 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { int fd; + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ fd = open(ACCEL_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } + + if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f3876..8f491acae 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -74,6 +74,10 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> @@ -84,6 +88,7 @@ #include <uORB/topics/geofence_result.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/vtol_vehicle_status.h> + #include <uORB/topics/vehicle_land_detected.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -241,6 +246,13 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + +/** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); @@ -356,31 +368,31 @@ void print_status() const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: + case vehicle_status_s::ARMING_STATE_INIT: armed_str = "INIT"; break; - case ARMING_STATE_STANDBY: + case vehicle_status_s::ARMING_STATE_STANDBY: armed_str = "STANDBY"; break; - case ARMING_STATE_ARMED: + case vehicle_status_s::ARMING_STATE_ARMED: armed_str = "ARMED"; break; - case ARMING_STATE_ARMED_ERROR: + case vehicle_status_s::ARMING_STATE_ARMED_ERROR: armed_str = "ARMED_ERROR"; break; - case ARMING_STATE_STANDBY_ERROR: + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: armed_str = "STANDBY_ERROR"; break; - case ARMING_STATE_REBOOT: + case vehicle_status_s::ARMING_STATE_REBOOT: armed_str = "REBOOT"; break; - case ARMING_STATE_IN_AIR_RESTORE: + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: armed_str = "IN_AIR_RESTORE"; break; @@ -403,7 +415,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { @@ -440,7 +452,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s transition_result_t main_ret = TRANSITION_NOT_CHANGED; /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF; transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state @@ -450,43 +462,43 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL); } } } @@ -513,7 +525,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s // Flick to inair restore first if this comes from an onboard system if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { - status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; + status_local->arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; } transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); @@ -538,12 +550,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -560,7 +572,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 0.5f) { //XXX update state machine? @@ -656,14 +668,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_NAV_GUIDED_ENABLE: { transition_result_t res = TRANSITION_DENIED; - static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + static main_state_t main_state_pre_offboard =vehicle_status_s::MAIN_STATE_MANUAL; - if (status_local->main_state != MAIN_STATE_OFFBOARD) { + if (status_local->main_state !=vehicle_status_s::MAIN_STATE_OFFBOARD) { main_state_pre_offboard = status_local->main_state; } if (cmd->param1 > 0.5f) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -716,6 +728,53 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s return true; } +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +{ + //Need global position fix to be able to set home + if (!status.condition_global_position_valid) { + return; + } + + //Ensure that the GPS accuracy is good enough for intializing home + if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + return; + } + + //Set home position + home.timestamp = hrt_absolute_time(); + home.lat = globalPosition.lat; + home.lon = globalPosition.lon; + home.alt = globalPosition.alt; + + home.x = localPosition.x; + home.y = localPosition.y; + home.z = localPosition.z; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (homePub > 0) { + orb_publish(ORB_ID(home_position), homePub, &home); + + } else { + homePub = orb_advertise(ORB_ID(home_position), &home); + } + + //Play tune first time we initialize HOME + if (!status.condition_home_position_valid) { + tune_positive(true); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; +} + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -739,41 +798,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - const char *main_states_str[MAIN_STATE_MAX]; - main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; - main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; - main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; - main_states_str[MAIN_STATE_ACRO] = "ACRO"; - main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; - - const char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[ARMING_STATE_INIT] = "INIT"; - arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; - arming_states_str[ARMING_STATE_ARMED] = "ARMED"; - arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; - arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - - const char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; - nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; - nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; - nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; - nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; - nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; - nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; + const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; + main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[vehicle_status_s::MAIN_STATE_ACRO] = "ACRO"; + main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD] = "OFFBOARD"; + + const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX]; + arming_states_str[vehicle_status_s::ARMING_STATE_INIT] = "INIT"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[vehicle_status_s::ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[vehicle_status_s::ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -794,10 +853,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = true; // initialize to safe value // We want to accept RC inputs as default status.rc_input_blocked = false; - status.main_state = MAIN_STATE_MANUAL; - status.nav_state = NAVIGATION_STATE_MANUAL; - status.arming_state = ARMING_STATE_INIT; - status.hil_state = HIL_STATE_OFF; + status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + status.arming_state = vehicle_status_s::ARMING_STATE_INIT; + status.hil_state = vehicle_status_s::HIL_STATE_OFF; status.failsafe = false; /* neither manual nor offboard control commands have been received */ @@ -810,7 +869,7 @@ int commander_thread_main(int argc, char *argv[]) status.data_link_lost = true; /* set battery warning flag */ - status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized @@ -904,7 +963,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; hrt_abstime last_idle_time = 0; - hrt_abstime start_time = 0; bool status_changed = true; bool param_init_forced = true; @@ -964,6 +1022,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); + /* Subscribe to land detector */ + int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + struct vehicle_land_detected_s land_detector; + memset(&land_detector, 0, sizeof(land_detector)); + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a @@ -1035,7 +1098,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; - start_time = hrt_absolute_time(); + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1080,14 +1143,14 @@ int commander_thread_main(int argc, char *argv[]) } /* disable manual override for all systems that rely on electronic stabilization */ - if (status.system_type == VEHICLE_TYPE_COAXIAL || - status.system_type == VEHICLE_TYPE_HELICOPTER || - status.system_type == VEHICLE_TYPE_TRICOPTER || - status.system_type == VEHICLE_TYPE_QUADROTOR || - status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR || - (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + if (status.system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER || + status.system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR || + status.system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { status.is_rotary_wing = true; @@ -1096,8 +1159,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1247,9 +1310,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + if (status.hil_state == vehicle_status_s::HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { @@ -1259,12 +1322,12 @@ int commander_thread_main(int argc, char *argv[]) } //Notify the user if the status of the safety switch changes - if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { - if(safety.safety_off) { + if (safety.safety_off) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); - } - else { + + } else { tune_neutral(true); } @@ -1279,8 +1342,9 @@ int commander_thread_main(int argc, char *argv[]) /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; + /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ - if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + if ((status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } @@ -1325,34 +1389,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* update home position */ - if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -1379,9 +1415,15 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + /* Update land detector */ + orb_check(land_detector_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + } + if (status.condition_local_altitude_valid) { - if (status.condition_landed != local_position.landed) { - status.condition_landed = local_position.landed; + if (status.condition_landed != land_detector.landed) { + status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { @@ -1401,7 +1443,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; @@ -1419,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("subsystem changed: %d\n", (int)info.subsystem_type); + //warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -1474,7 +1516,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f @@ -1482,10 +1524,10 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1493,7 +1535,7 @@ int commander_thread_main(int argc, char *argv[]) } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1507,9 +1549,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1609,7 +1651,8 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums", + (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -1619,14 +1662,14 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && + (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && + (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : - ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY : + vehicle_status_s::ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1645,7 +1688,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && + if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1653,11 +1696,11 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if (status.main_state != MAIN_STATE_MANUAL) { + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1676,7 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) } if (arming_ret == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { @@ -1710,9 +1753,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; - status.rc_signal_lost_timestamp=sp_man.timestamp; + status.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; } } @@ -1814,10 +1857,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the data link and the gps system have been checked * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ - if (status.main_state != MAIN_STATE_MANUAL && - status.main_state != MAIN_STATE_ACRO && - status.main_state != MAIN_STATE_ALTCTL && - status.main_state != MAIN_STATE_POSCTL && + if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && + status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && + status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && ((status.data_link_lost && status.gps_failure) || (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1838,10 +1881,10 @@ int commander_thread_main(int argc, char *argv[]) /* At this point the rc signal and the gps system have been checked * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && + if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || + status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || + status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && ((status.rc_signal_lost && status.gps_failure) || (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; @@ -1859,42 +1902,23 @@ int commander_thread_main(int argc, char *argv[]) } } + //Get current timestamp + const hrt_abstime now = hrt_absolute_time(); - hrt_abstime t1 = hrt_absolute_time(); + //First time home position update + if (!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); + } /* print new state */ if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - // TODO remove code duplication - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - } - arming_state_changed = false; } @@ -1915,11 +1939,14 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; + if (status.failsafe) { mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { mavlink_log_critical(mavlink_fd, "failsafe mode off"); } + failsafe_old = status.failsafe; } @@ -1932,13 +1959,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); - control_mode.timestamp = t1; + control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - status.timestamp = t1; + status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed.timestamp = t1; + armed.timestamp = now; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1949,11 +1976,11 @@ int commander_thread_main(int argc, char *argv[]) set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { + } else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1965,7 +1992,7 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { //Notify the user that it is safe to approach the vehicle - if(arm_tune_played) { + if (arm_tune_played) { tune_neutral(true); } @@ -2045,15 +2072,15 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu bool set_normal_color = false; /* set mode */ - if (status_local->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); rgbled_set_color(RGBLED_COLOR_RED); - } else if (status_local->arming_state == ARMING_STATE_STANDBY) { + } else if (status_local->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -2064,9 +2091,9 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu if (set_normal_color) { /* set color */ - if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { + if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); - /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { if (status_local->condition_local_position_valid) { @@ -2122,12 +2149,12 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* if offboard is set allready by a mavlink command, abort */ if (status.offboard_control_set_by_command) { - return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* offboard switch overrides main switch */ - if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -2139,24 +2166,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { - case SWITCH_POS_NONE: + case manual_control_setpoint_s::SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; break; - case SWITCH_POS_OFF: // MANUAL - if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_ACRO); + case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL + if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO); } else { - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST + if (sp_man->posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2166,24 +2193,24 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctl_switch != SWITCH_POS_ON) { + if (sp_man->posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) { print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_ON: // AUTO - if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); + case manual_control_setpoint_s::SWITCH_POS_ON: // AUTO + if (sp_man->return_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2192,14 +2219,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + } else if (sp_man->loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2208,7 +2235,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2217,7 +2244,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -2225,21 +2252,21 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status_local, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status_local, MAIN_STATE_MANUAL); + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -2256,11 +2283,11 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); - control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_system_hil_enabled = status.hil_state == vehicle_status_s::HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); @@ -2272,7 +2299,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2284,7 +2311,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2296,12 +2323,12 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RCRECOVER: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2313,7 +2340,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2325,7 +2352,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -2338,7 +2365,7 @@ set_control_mode() break; - case NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2351,7 +2378,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: /* TODO: check if this makes sense */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2364,7 +2391,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2377,7 +2404,7 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_offboard_enabled = true; @@ -2432,6 +2459,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; default: @@ -2470,7 +2498,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: @@ -2575,7 +2603,7 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; @@ -2639,7 +2667,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 2022e99fb..8a4451100 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -72,16 +72,16 @@ static const int ERROR = -1; bool is_multirotor(const struct vehicle_status_s *current_status) { - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + return ((current_status->system_type == vehicle_status_s::VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + return is_multirotor(current_status) || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_COAXIAL); } static int buzzer = -1; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 3cfa8b4c6..4ddb4e0fb 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -73,7 +73,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool armed; // actuator_armed_s.armed bool ready_to_arm; // actuator_armed_s.ready_to_arm } ArmingTransitionVolatileState_t; - + // This structure represents a test case for arming_state_transition. It contains the machine // state prior to transtion, the requested state to transition to and finally the expected // machine state after transition. @@ -88,7 +88,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition transition_result_t expected_transition_result; // Expected result from arming_state_transition } ArmingTransitionTest_t; - + // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false @@ -100,182 +100,182 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) #define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_OFF true #define ATT_SAFETY_ON false - + // These are test cases for arming_state_transition static const ArmingTransitionTest_t rgArmingTransitionTests[] = { // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - + { "transition: init to standby", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to standby error", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: init to reboot", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to init", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to standby error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to reboot", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to standby", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed to armed error", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: armed error to standby error", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY_ERROR, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby error to reboot", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to armed", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: in air restore to reboot", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // hil on tests, standby error to standby not normally allowed - + { "transition: standby error to standby, hil on", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // Safety switch arming tests - + { "transition: standby to armed, no safety switch", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + { "transition: standby to armed, safety switch off", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + // standby error { "transition: armed error to standby error requested standby", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + // TRANSITION_DENIED tests - + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - + { "no transition: init to armed", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby to armed error", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED_ERROR, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED_ERROR, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to init", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_INIT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_INIT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed to reboot", - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to armed", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: armed error to reboot", - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_REBOOT, - { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_REBOOT, + { vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to armed", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: standby error to standby", - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: reboot to armed", - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + { "no transition: in air restore to standby", - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Sensor tests - + { "no transition: init to standby - sensors not initialized", - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_STANDBY, - { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_STANDBY, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + // Safety switch arming tests - + { "no transition: init to standby, safety switch on", - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - ARMING_STATE_ARMED, - { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + vehicle_status_s::ARMING_STATE_ARMED, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, }; - + struct vehicle_status_s status; struct safety_s safety; struct actuator_armed_s armed; - + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); for (size_t i=0; i<cArmingTransitionTests; i++) { const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i]; - + // Setup initial machine state status.arming_state = test->current_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; @@ -286,10 +286,10 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; - + // Attempt transition transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); @@ -310,7 +310,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) main_state_t to_state; // State to transition to transition_result_t expected_transition_result; // Expected result from main_state_transition call } MainTransitionTest_t; - + // Bits for condition_bits #define MTT_ALL_NOT_VALID 0 #define MTT_ROTARY_WING 1 << 0 @@ -318,108 +318,108 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) #define MTT_LOC_POS_VALID 1 << 2 #define MTT_HOME_POS_VALID 1 << 3 #define MTT_GLOBAL_POS_VALID 1 << 4 - + static const MainTransitionTest_t rgMainTransitionTests[] = { - + // TRANSITION_NOT_CHANGED tests - + { "no transition: identical states", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + // TRANSITION_CHANGED tests - + { "transition: MANUAL to ACRO", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, { "transition: ACRO to MANUAL", MTT_ALL_NOT_VALID, - MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, { "transition: AUTO_LOITER to MANUAL - global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, - MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - not rotary", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", MTT_ROTARY_WING | MTT_LOC_ALT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + { "transition: ALTCTL to MANUAL - local altitude valid", MTT_LOC_ALT_VALID, - MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position not valid, global position valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: MANUAL to POSCTL - local position valid, global position not valid", MTT_LOC_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, { "transition: POSCTL to MANUAL - local position valid, global position valid", MTT_LOC_POS_VALID, - MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, // TRANSITION_DENIED tests { "no transition: MANUAL to AUTO_MISSION - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_LOITER - global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", MTT_HOME_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", MTT_GLOBAL_POS_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", MTT_ROTARY_WING, - MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, - + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", MTT_ALL_NOT_VALID, - MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, }; - + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); for (size_t i=0; i<cMainTransitionTests; i++) { const MainTransitionTest_t* test = &rgMainTransitionTests[i]; @@ -432,10 +432,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; - + // Attempt transition transition_result_t result = main_state_transition(¤t_state, test->to_state); - + // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); if (test->expected_transition_result == result) { @@ -495,8 +495,8 @@ bool StateMachineHelperTest::run_tests(void) ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); - + return (_tests_failed == 0); } -ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8ab14dd52..8410297ef 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -62,6 +62,7 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "HOLD STILL"); @@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); @@ -95,7 +99,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); + int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -107,7 +111,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; @@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } + if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7be8de9c6..2afb9a440 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,7 @@ static const char *sensor_name = "mag"; int do_mag_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); @@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd) /* erase old calibration */ int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (res != OK) { @@ -145,55 +149,61 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; + int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0); + + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + calibration_counter = 0U; - calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + int poll_ret = poll(fds, 1, 1000); - int poll_ret = poll(fds, 1, 1000); + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + calibration_counter++; - calibration_counter++; + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -201,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); @@ -253,6 +263,9 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ + if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + res = ERROR; + } if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 465f9cdc5..40da9c77b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -76,19 +76,19 @@ static const int ERROR = -1; // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. -static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { +static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char * const state_names[ARMING_STATE_MAX] = { +static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -107,8 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none { // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); + ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1); transition_result_t ret = TRANSITION_DENIED; arming_state_t current_arming_state = status->arming_state; @@ -126,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s int prearm_ret = OK; /* only perform the check if we have to */ - if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) { + if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { prearm_ret = prearm_check(status, mavlink_fd); } @@ -136,7 +136,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s irqstate_t flags = irqsave(); /* enforce lockdown in HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { armed->lockdown = true; } else { @@ -148,12 +148,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s if (valid_transition) { // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { // Do not perform pre-arm checks if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && - status->hil_state == HIL_STATE_OFF) { + // Allow if vehicle_status_s::HIL_STATE_ON + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == vehicle_status_s::HIL_STATE_OFF) { // Fail transition if pre-arm check fails if (prearm_ret) { @@ -200,18 +200,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; + } else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { + new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } } // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { valid_transition = true; } /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); feedback_provided = true; valid_transition = false; @@ -219,8 +219,8 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Finish up the state transition if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; } @@ -264,12 +264,12 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { - case MAIN_STATE_MANUAL: - case MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || @@ -279,7 +279,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { @@ -287,22 +287,22 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_AUTO_MISSION: - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* need offboard signal */ if (!status->offboard_control_signal_lost) { @@ -311,7 +311,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_MAX: + case vehicle_status_s::MAIN_STATE_MAX: default: break; } @@ -338,16 +338,16 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } else { switch (new_state) { - case HIL_STATE_OFF: + case vehicle_status_s::HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; - case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { + case vehicle_status_s::HIL_STATE_ON: + if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY + || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { /* Disable publication of all attached sensors */ /* list directory */ @@ -448,55 +448,55 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en { navigation_state_t nav_state_old = status->nav_state; - bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ switch (status->main_state) { - case MAIN_STATE_ACRO: - case MAIN_STATE_MANUAL: - case MAIN_STATE_ALTCTL: - case MAIN_STATE_POSCTL: + case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_ALTCTL: + case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { switch (status->main_state) { - case MAIN_STATE_ACRO: - status->nav_state = NAVIGATION_STATE_ACRO; + case vehicle_status_s::MAIN_STATE_ACRO: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO; break; - case MAIN_STATE_MANUAL: - status->nav_state = NAVIGATION_STATE_MANUAL; + case vehicle_status_s::MAIN_STATE_MANUAL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; - case MAIN_STATE_ALTCTL: - status->nav_state = NAVIGATION_STATE_ALTCTL; + case vehicle_status_s::MAIN_STATE_ALTCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; - case MAIN_STATE_POSCTL: - status->nav_state = NAVIGATION_STATE_POSCTL; + case vehicle_status_s::MAIN_STATE_POSCTL: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; break; default: - status->nav_state = NAVIGATION_STATE_MANUAL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; } } break; - case MAIN_STATE_AUTO_MISSION: + case vehicle_status_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe * - if commanded to do so @@ -505,19 +505,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* first look at the commands */ if (status->engine_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->gps_failure_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* datalink loss enabled: * check for datalink lost: this should always trigger RTGS */ @@ -525,13 +525,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* datalink loss disabled: @@ -542,37 +542,37 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; } break; - case MAIN_STATE_AUTO_LOITER: + case vehicle_status_s::MAIN_STATE_AUTO_LOITER: /* go into failsafe on a engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* go into failsafe if RC is lost and datalink loss is not set up */ @@ -580,65 +580,65 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } /* don't bother if RC is lost if datalink is connected */ } else if (status->rc_signal_lost) { /* this mode is ok, we don't need RC for loitering */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } else { /* everything is perfect */ - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; } break; - case MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_RTL: /* require global position and home, also go into failsafe on an engine failure */ if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } break; - case MAIN_STATE_OFFBOARD: + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { status->failsafe = true; - status->nav_state = NAVIGATION_STATE_POSCTL; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { status->failsafe = true; if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; } } else { - status->nav_state = NAVIGATION_STATE_OFFBOARD; + status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD; } default: break; diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index a8a70507e..491d4681d 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -47,6 +47,10 @@ #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/parameter_update.h> #include <stdio.h> diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 705e54be3..68bf12024 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -793,7 +793,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 968270847..38660b433 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 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 @@ -82,12 +82,11 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mavlink/mavlink_log.h> #include <platforms/px4_defines.h> -#include "estimator_23states.h" - - +#include "estimator_22states.h" /** * estimator app start / stop handling function @@ -102,7 +101,7 @@ __EXPORT uint64_t getMicros(); static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds uint32_t millis() { @@ -223,8 +222,10 @@ private: float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ + hrt_abstime _last_debug_print = 0; perf_counter_t _loop_perf; ///< loop performance counter + perf_counter_t _loop_intvl; ///< loop rate counter perf_counter_t _perf_gyro; ///<local performance counter for gyro updates perf_counter_t _perf_mag; ///<local performance counter for mag updates perf_counter_t _perf_gps; ///<local performance counter for gps updates @@ -291,10 +292,6 @@ private: AttPosEKF *_ekf; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - /** * Update our local parameter cache. */ @@ -400,6 +397,7 @@ FixedwingEstimator::FixedwingEstimator() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), @@ -423,10 +421,7 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _parameters{}, _parameter_handles{}, - _ekf(nullptr), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f) + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -725,7 +720,7 @@ FixedwingEstimator::task_main() * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -783,6 +778,11 @@ FixedwingEstimator::task_main() _gps.vel_e_m_s = 0.0f; _gps.vel_d_m_s = 0.0f; + // init lowpass filters for baro and gps altitude + float _gps_alt_filt = 0, _baro_alt_filt = 0; + float rc = 10.0f; // RC time constant of 1st order LPF in seconds + hrt_abstime baro_last = 0; + _task_running = true; while (!_task_should_exit) { @@ -801,6 +801,7 @@ FixedwingEstimator::task_main() } perf_begin(_loop_perf); + perf_count(_loop_intvl); /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { @@ -816,7 +817,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ - bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); + bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -825,7 +826,7 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ - if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); @@ -1056,6 +1057,11 @@ FixedwingEstimator::task_main() _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + // update LPF + _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); + + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { @@ -1071,7 +1077,6 @@ FixedwingEstimator::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; - last_gps = _gps.timestamp_position; } @@ -1083,15 +1088,15 @@ FixedwingEstimator::task_main() if (baro_updated) { - hrt_abstime baro_last = _baro.timestamp; - - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + baro_last = _baro.timestamp; _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1181,6 +1186,24 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1194,9 +1217,13 @@ FixedwingEstimator::task_main() initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - _baro_gps_offset = _baro.altitude - gps_alt; + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _baro_alt_filt = _baro.altitude; + _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1372,10 +1399,15 @@ FixedwingEstimator::task_main() } if (newRangeData) { - _ekf->fuseRngData = true; - _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->GroundEKF(); + + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } } @@ -1435,22 +1467,6 @@ FixedwingEstimator::task_main() _local_pos.v_z_valid = true; _local_pos.xy_global = true; - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; - - - /* crude land detector for fixedwing only, - * TODO: adapt so that it works for both, maybe move to another location - */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } - _local_pos.z_global = false; _local_pos.yaw = _att.yaw; @@ -1515,8 +1531,8 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->windSpdFiltNorth; - _wind.windspeed_east = _ekf->windSpdFiltEast; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; // XXX we need to do something smart about the covariance here // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp deleted file mode 100644 index 67bfec4ea..000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ /dev/null @@ -1,2142 +0,0 @@ -#include "estimator_21states.h" - -#include <string.h> - -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - EAS2TAS(1.0f), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - numericalProtection(true), - storeIndex(0), - magDeclination(0.0f) -{ - InitialiseParameters(); -} - -AttPosEKF::~AttPosEKF() -{ -} - -void AttPosEKF::UpdateStrapdownEquationsNED() -{ - Vector3f delVelNav; - float q00; - float q11; - float q22; - float q33; - float q01; - float q02; - float q03; - float q12; - float q13; - float q23; - Mat3f Tbn; - Mat3f Tnb; - float rotationMag; - float qUpdated[4]; - float quatMag; - double deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - -// Remove sensor bias errors - correctedDelAng.x = dAngIMU.x - states[10]; - correctedDelAng.y = dAngIMU.y - states[11]; - correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; - -// Save current measurements - Vector3f prevDelAng = correctedDelAng; - -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion - rotationMag = correctedDelAng.length(); - if (rotationMag < 1e-12f) - { - deltaQuat[0] = 1.0; - deltaQuat[1] = 0.0; - deltaQuat[2] = 0.0; - deltaQuat[3] = 0.0; - } - else - { - deltaQuat[0] = cos(0.5f*rotationMag); - double rotScaler = (sin(0.5f*rotationMag))/rotationMag; - deltaQuat[1] = correctedDelAng.x*rotScaler; - deltaQuat[2] = correctedDelAng.y*rotScaler; - deltaQuat[3] = correctedDelAng.z*rotScaler; - } - -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion - qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; - qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; - qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; - qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - -// Normalise the quaternions and update the quaternion states - quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); - if (quatMag > 1e-16f) - { - float quatMagInv = 1.0f/quatMag; - states[0] = quatMagInv*qUpdated[0]; - states[1] = quatMagInv*qUpdated[1]; - states[2] = quatMagInv*qUpdated[2]; - states[3] = quatMagInv*qUpdated[3]; - } - -// Calculate the body to nav cosine matrix - q00 = sq(states[0]); - q11 = sq(states[1]); - q22 = sq(states[2]); - q33 = sq(states[3]); - q01 = states[0]*states[1]; - q02 = states[0]*states[2]; - q03 = states[0]*states[3]; - q12 = states[1]*states[2]; - q13 = states[1]*states[3]; - q23 = states[2]*states[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); - - Tnb = Tbn.transpose(); - -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded - //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) - accNavMag = delVelNav.length()/dtIMU; - -// If calculating position save previous velocity - float lastVelocity[3]; - lastVelocity[0] = states[4]; - lastVelocity[1] = states[5]; - lastVelocity[2] = states[6]; - -// Sum delta velocities to get velocity - states[4] = states[4] + delVelNav.x; - states[5] = states[5] + delVelNav.y; - states[6] = states[6] + delVelNav.z; - -// If calculating postions, do a trapezoidal integration for position - states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; - states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; - states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - - // Constrain states (to protect against filter divergence) - //ConstrainStates(); -} - -void AttPosEKF::CovariancePrediction(float dt) -{ - // scalars - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - - // arrays - float processNoise[21]; - float SF[14]; - float SG[8]; - float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; - - // calculate covariance prediction process noise - windVelSigma = dt*0.1f; - dAngBiasSigma = dt*5.0e-7f; - magEarthSigma = dt*3.0e-4f; - magBodySigma = dt*3.0e-4f; - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; - for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; - for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; - for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; - for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - - // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); - if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); - - // Predicted covariance calculation - SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; - SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SF[3] = day/2 - day_b/2; - SF[4] = daz/2 - daz_b/2; - SF[5] = dax/2 - dax_b/2; - SF[6] = dax_b/2 - dax/2; - SF[7] = daz_b/2 - daz/2; - SF[8] = day_b/2 - day/2; - SF[9] = q1/2; - SF[10] = q2/2; - SF[11] = q3/2; - SF[12] = 2*dvz*q0; - SF[13] = 2*dvy*q1; - - SG[0] = q0/2; - SG[1] = sq(q3); - SG[2] = sq(q2); - SG[3] = sq(q1); - SG[4] = sq(q0); - SG[5] = 2*q2*q3; - SG[6] = 2*q1*q3; - SG[7] = 2*q1*q2; - - SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); - SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); - SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); - SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; - SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; - SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; - SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; - SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; - SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; - SQ[9] = sq(SG[0]); - SQ[10] = sq(q1); - - SPP[0] = SF[12] + SF[13] - 2*dvx*q2; - SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SPP[3] = SF[11]; - SPP[4] = SF[10]; - SPP[5] = SF[9]; - SPP[6] = SF[7]; - SPP[7] = SF[8]; - - nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); - nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); - nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); - nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); - nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); - nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); - nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; - nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; - nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; - nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; - nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; - nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; - nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; - nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; - nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; - nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; - nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); - nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); - nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); - nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); - nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); - nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; - nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; - nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; - nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; - nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; - nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; - nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; - nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; - nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; - nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; - nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; - nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); - nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); - nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); - nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); - nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); - nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); - nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; - nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; - nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; - nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; - nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; - nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; - nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; - nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; - nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; - nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; - nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; - nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); - nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); - nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); - nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); - nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); - nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; - nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; - nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; - nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; - nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; - nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; - nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; - nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; - nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; - nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; - nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); - nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); - nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); - nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); - nextP[7][10] = P[7][10] + P[4][10]*dt; - nextP[7][11] = P[7][11] + P[4][11]*dt; - nextP[7][12] = P[7][12] + P[4][12]*dt; - nextP[7][13] = P[7][13] + P[4][13]*dt; - nextP[7][14] = P[7][14] + P[4][14]*dt; - nextP[7][15] = P[7][15] + P[4][15]*dt; - nextP[7][16] = P[7][16] + P[4][16]*dt; - nextP[7][17] = P[7][17] + P[4][17]*dt; - nextP[7][18] = P[7][18] + P[4][18]*dt; - nextP[7][19] = P[7][19] + P[4][19]*dt; - nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); - nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); - nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); - nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); - nextP[8][10] = P[8][10] + P[5][10]*dt; - nextP[8][11] = P[8][11] + P[5][11]*dt; - nextP[8][12] = P[8][12] + P[5][12]*dt; - nextP[8][13] = P[8][13] + P[5][13]*dt; - nextP[8][14] = P[8][14] + P[5][14]*dt; - nextP[8][15] = P[8][15] + P[5][15]*dt; - nextP[8][16] = P[8][16] + P[5][16]*dt; - nextP[8][17] = P[8][17] + P[5][17]*dt; - nextP[8][18] = P[8][18] + P[5][18]*dt; - nextP[8][19] = P[8][19] + P[5][19]*dt; - nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); - nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); - nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); - nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); - nextP[9][10] = P[9][10] + P[6][10]*dt; - nextP[9][11] = P[9][11] + P[6][11]*dt; - nextP[9][12] = P[9][12] + P[6][12]*dt; - nextP[9][13] = P[9][13] + P[6][13]*dt; - nextP[9][14] = P[9][14] + P[6][14]*dt; - nextP[9][15] = P[9][15] + P[6][15]*dt; - nextP[9][16] = P[9][16] + P[6][16]*dt; - nextP[9][17] = P[9][17] + P[6][17]*dt; - nextP[9][18] = P[9][18] + P[6][18]*dt; - nextP[9][19] = P[9][19] + P[6][19]*dt; - nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; - nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; - nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; - nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; - nextP[10][7] = P[10][7] + P[10][4]*dt; - nextP[10][8] = P[10][8] + P[10][5]*dt; - nextP[10][9] = P[10][9] + P[10][6]*dt; - nextP[10][10] = P[10][10]; - nextP[10][11] = P[10][11]; - nextP[10][12] = P[10][12]; - nextP[10][13] = P[10][13]; - nextP[10][14] = P[10][14]; - nextP[10][15] = P[10][15]; - nextP[10][16] = P[10][16]; - nextP[10][17] = P[10][17]; - nextP[10][18] = P[10][18]; - nextP[10][19] = P[10][19]; - nextP[10][20] = P[10][20]; - nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; - nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; - nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; - nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; - nextP[11][7] = P[11][7] + P[11][4]*dt; - nextP[11][8] = P[11][8] + P[11][5]*dt; - nextP[11][9] = P[11][9] + P[11][6]*dt; - nextP[11][10] = P[11][10]; - nextP[11][11] = P[11][11]; - nextP[11][12] = P[11][12]; - nextP[11][13] = P[11][13]; - nextP[11][14] = P[11][14]; - nextP[11][15] = P[11][15]; - nextP[11][16] = P[11][16]; - nextP[11][17] = P[11][17]; - nextP[11][18] = P[11][18]; - nextP[11][19] = P[11][19]; - nextP[11][20] = P[11][20]; - nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; - nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; - nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; - nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; - nextP[12][7] = P[12][7] + P[12][4]*dt; - nextP[12][8] = P[12][8] + P[12][5]*dt; - nextP[12][9] = P[12][9] + P[12][6]*dt; - nextP[12][10] = P[12][10]; - nextP[12][11] = P[12][11]; - nextP[12][12] = P[12][12]; - nextP[12][13] = P[12][13]; - nextP[12][14] = P[12][14]; - nextP[12][15] = P[12][15]; - nextP[12][16] = P[12][16]; - nextP[12][17] = P[12][17]; - nextP[12][18] = P[12][18]; - nextP[12][19] = P[12][19]; - nextP[12][20] = P[12][20]; - nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; - nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; - nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; - nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; - nextP[13][7] = P[13][7] + P[13][4]*dt; - nextP[13][8] = P[13][8] + P[13][5]*dt; - nextP[13][9] = P[13][9] + P[13][6]*dt; - nextP[13][10] = P[13][10]; - nextP[13][11] = P[13][11]; - nextP[13][12] = P[13][12]; - nextP[13][13] = P[13][13]; - nextP[13][14] = P[13][14]; - nextP[13][15] = P[13][15]; - nextP[13][16] = P[13][16]; - nextP[13][17] = P[13][17]; - nextP[13][18] = P[13][18]; - nextP[13][19] = P[13][19]; - nextP[13][20] = P[13][20]; - nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; - nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; - nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; - nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; - nextP[14][7] = P[14][7] + P[14][4]*dt; - nextP[14][8] = P[14][8] + P[14][5]*dt; - nextP[14][9] = P[14][9] + P[14][6]*dt; - nextP[14][10] = P[14][10]; - nextP[14][11] = P[14][11]; - nextP[14][12] = P[14][12]; - nextP[14][13] = P[14][13]; - nextP[14][14] = P[14][14]; - nextP[14][15] = P[14][15]; - nextP[14][16] = P[14][16]; - nextP[14][17] = P[14][17]; - nextP[14][18] = P[14][18]; - nextP[14][19] = P[14][19]; - nextP[14][20] = P[14][20]; - nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; - nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; - nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; - nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; - nextP[15][7] = P[15][7] + P[15][4]*dt; - nextP[15][8] = P[15][8] + P[15][5]*dt; - nextP[15][9] = P[15][9] + P[15][6]*dt; - nextP[15][10] = P[15][10]; - nextP[15][11] = P[15][11]; - nextP[15][12] = P[15][12]; - nextP[15][13] = P[15][13]; - nextP[15][14] = P[15][14]; - nextP[15][15] = P[15][15]; - nextP[15][16] = P[15][16]; - nextP[15][17] = P[15][17]; - nextP[15][18] = P[15][18]; - nextP[15][19] = P[15][19]; - nextP[15][20] = P[15][20]; - nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; - nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; - nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; - nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; - nextP[16][7] = P[16][7] + P[16][4]*dt; - nextP[16][8] = P[16][8] + P[16][5]*dt; - nextP[16][9] = P[16][9] + P[16][6]*dt; - nextP[16][10] = P[16][10]; - nextP[16][11] = P[16][11]; - nextP[16][12] = P[16][12]; - nextP[16][13] = P[16][13]; - nextP[16][14] = P[16][14]; - nextP[16][15] = P[16][15]; - nextP[16][16] = P[16][16]; - nextP[16][17] = P[16][17]; - nextP[16][18] = P[16][18]; - nextP[16][19] = P[16][19]; - nextP[16][20] = P[16][20]; - nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; - nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; - nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; - nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; - nextP[17][7] = P[17][7] + P[17][4]*dt; - nextP[17][8] = P[17][8] + P[17][5]*dt; - nextP[17][9] = P[17][9] + P[17][6]*dt; - nextP[17][10] = P[17][10]; - nextP[17][11] = P[17][11]; - nextP[17][12] = P[17][12]; - nextP[17][13] = P[17][13]; - nextP[17][14] = P[17][14]; - nextP[17][15] = P[17][15]; - nextP[17][16] = P[17][16]; - nextP[17][17] = P[17][17]; - nextP[17][18] = P[17][18]; - nextP[17][19] = P[17][19]; - nextP[17][20] = P[17][20]; - nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; - nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; - nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; - nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; - nextP[18][7] = P[18][7] + P[18][4]*dt; - nextP[18][8] = P[18][8] + P[18][5]*dt; - nextP[18][9] = P[18][9] + P[18][6]*dt; - nextP[18][10] = P[18][10]; - nextP[18][11] = P[18][11]; - nextP[18][12] = P[18][12]; - nextP[18][13] = P[18][13]; - nextP[18][14] = P[18][14]; - nextP[18][15] = P[18][15]; - nextP[18][16] = P[18][16]; - nextP[18][17] = P[18][17]; - nextP[18][18] = P[18][18]; - nextP[18][19] = P[18][19]; - nextP[18][20] = P[18][20]; - nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; - nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; - nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; - nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; - nextP[19][7] = P[19][7] + P[19][4]*dt; - nextP[19][8] = P[19][8] + P[19][5]*dt; - nextP[19][9] = P[19][9] + P[19][6]*dt; - nextP[19][10] = P[19][10]; - nextP[19][11] = P[19][11]; - nextP[19][12] = P[19][12]; - nextP[19][13] = P[19][13]; - nextP[19][14] = P[19][14]; - nextP[19][15] = P[19][15]; - nextP[19][16] = P[19][16]; - nextP[19][17] = P[19][17]; - nextP[19][18] = P[19][18]; - nextP[19][19] = P[19][19]; - nextP[19][20] = P[19][20]; - nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; - nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; - nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; - nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; - nextP[20][7] = P[20][7] + P[20][4]*dt; - nextP[20][8] = P[20][8] + P[20][5]*dt; - nextP[20][9] = P[20][9] + P[20][6]*dt; - nextP[20][10] = P[20][10]; - nextP[20][11] = P[20][11]; - nextP[20][12] = P[20][12]; - nextP[20][13] = P[20][13]; - nextP[20][14] = P[20][14]; - nextP[20][15] = P[20][15]; - nextP[20][16] = P[20][16]; - nextP[20][17] = P[20][17]; - nextP[20][18] = P[20][18]; - nextP[20][19] = P[20][19]; - nextP[20][20] = P[20][20]; - - for (unsigned i = 0; i < n_states; i++) - { - nextP[i][i] = nextP[i][i] + processNoise[i]; - } - - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); - } - - // If the total position variance exceds 1E6 (1000m), then stop covariance - // growth by setting the predicted to the previous values - // This prevent an ill conditioned matrix from occurring for long periods - // without GPS - if ((P[7][7] + P[8][8]) > 1E6f) - { - for (uint8_t i=7; i<=8; i++) - { - for (unsigned j = 0; j < n_states; j++) - { - nextP[i][j] = P[i][j]; - nextP[j][i] = P[j][i]; - } - } - } - - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i < 14; i++) { - P[i][i] = nextP[i][i]; - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 12) || (j > 12)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } - - } - - ConstrainVariances(); -} - -void AttPosEKF::FuseVelposNED() -{ - -// declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time - uint32_t horizRetryTime; - -// declare variables used to check measurement errors - float velInnov[3] = {0.0f,0.0f,0.0f}; - float posInnov[2] = {0.0f,0.0f}; - float hgtInnov = 0.0f; - -// declare variables used to control access to arrays - bool fuseData[6] = {false,false,false,false,false,false}; - uint8_t stateIndex; - uint8_t obsIndex; - uint8_t indexLimit; - -// declare variables used by state and covariance update calculations - float velErr; - float posErr; - float R_OBS[6]; - float observation[6]; - float SK; - float quatMag; - -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (fuseVelData || fusePosData || fuseHgtData) - { - // set the GPS data timeout depending on whether airspeed data is present - if (useAirspeed) horizRetryTime = gpsRetryTime; - else horizRetryTime = gpsRetryTimeNoTAS; - - // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; - observation[5] = -(hgtMea); - - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); - R_OBS[1] = R_OBS[0]; - R_OBS[2] = 0.08f + sq(velErr); - R_OBS[3] = R_OBS[2]; - R_OBS[4] = 4.0f + sq(posErr); - R_OBS[5] = 4.0f; - - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } - // calculate innovations and check GPS data validity using an innovation consistency check - if (fuseVelData) - { - // test velocity measurements - uint8_t imax = 2; - if (fusionModeGPS == 1) imax = 1; - for (uint8_t i = 0; i<=imax; i++) - { - velInnov[i] = statesAtVelTime[i+4] - velNED[i]; - stateIndex = 4 + i; - varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - } - // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { - current_ekf_state.velHealth = true; - current_ekf_state.velFailTime = millis(); - } - else - { - current_ekf_state.velHealth = false; - } - } - if (fusePosData) - { - // test horizontal position measurements - posInnov[0] = statesAtPosTime[7] - posNE[0]; - posInnov[1] = statesAtPosTime[8] - posNE[1]; - varInnovVelPos[3] = P[7][7] + R_OBS[3]; - varInnovVelPos[4] = P[8][8] + R_OBS[4]; - // apply a 10-sigma threshold - current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; - if (current_ekf_state.posHealth || current_ekf_state.posTimeout) - { - current_ekf_state.posHealth = true; - current_ekf_state.posFailTime = millis(); - } - else - { - current_ekf_state.posHealth = false; - } - } - // test height measurements - if (fuseHgtData) - { - hgtInnov = statesAtHgtTime[9] + hgtMea; - varInnovVelPos[5] = P[9][9] + R_OBS[5]; - // apply a 10-sigma threshold - current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) - { - current_ekf_state.hgtHealth = true; - current_ekf_state.hgtFailTime = millis(); - } - else - { - current_ekf_state.hgtHealth = false; - } - } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - fuseData[2] = true; - } - if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - } - if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) - { - fuseData[3] = true; - fuseData[4] = true; - } - if (fuseHgtData && current_ekf_state.hgtHealth) - { - fuseData[5] = true; - } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - // Fuse measurements sequentially - for (obsIndex=0; obsIndex<=5; obsIndex++) - { - if (fuseData[obsIndex]) - { - stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) - { - innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 3 || obsIndex == 4) - { - innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 5) - { - innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; - } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging - varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; - for (uint8_t i= 0; i<=indexLimit; i++) - { - Kfusion[i] = P[i][stateIndex]*SK; - } - // Calculate state corrections and re-normalise the quaternions - for (uint8_t i = 0; i<=indexLimit; i++) - { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) // divide by 0 protection - { - for (uint8_t i = 0; i<=3; i++) - { - states[i] = states[i] / quatMag; - } - } - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - KHP[i][j] = Kfusion[i] * P[stateIndex][j]; - } - } - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); - - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); -} - -void AttPosEKF::FuseMagnetometer() -{ - uint8_t obsIndex; - uint8_t indexLimit; - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; - float MagPred[3] = {0.0f,0.0f,0.0f}; - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) - { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - - static float R_MAG = 0.0025f; - - float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseMagData) - { - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - - // Copy required states to local variable names - q0 = statesAtMagMeasTime[0]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM[0][1] = 2*(q1*q2 + q0*q3); - DCM[0][2] = 2*(q1*q3-q0*q2); - DCM[1][0] = 2*(q1*q2 - q0*q3); - DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM[1][2] = 2*(q2*q3 + q0*q1); - DCM[2][0] = 2*(q1*q3 + q0*q2); - DCM[2][1] = 2*(q2*q3 - q0*q1); - DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; - MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; - MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - - // scale magnetometer observation error with total angular rate - R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; - SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2*magN*q0; - SH_MAG[8] = 2*magE*q3; - - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[1] = SH_MAG[0]; - H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; - H_MAG[3] = SH_MAG[2]; - H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[16] = 2*q0*q3 + 2*q1*q2; - H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1.0f; - - // Calculate Kalman gain - SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MX[4] = 2*q0*q2 - 2*q1*q3; - SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); - Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); - varInnovMag[0] = 1.0f/SK_MX[0]; - innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[2]; - H_MAG[1] = SH_MAG[1]; - H_MAG[2] = SH_MAG[0]; - H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; - H_MAG[15] = 2*q1*q2 - 2*q0*q3; - H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; - H_MAG[17] = 2*q0*q1 + 2*q2*q3; - H_MAG[19] = 1; - - // Calculate Kalman gain - SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; - SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MY[3] = 2*q0*q3 - 2*q1*q2; - SK_MY[4] = 2*q0*q1 + 2*q2*q3; - Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); - Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); - Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); - Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); - Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); - Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); - Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); - Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); - Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); - Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); - Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); - Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); - Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); - Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); - Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); - Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); - Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); - Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); - Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); - Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); - Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); - varInnovMag[1] = 1.0f/SK_MY[0]; - innovMag[1] = MagPred[1] - magData.y; - } - else if (obsIndex == 2) // we are now fusing the Z measurement - { - // Calculate observation jacobians - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[1]; - H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; - H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[3] = SH_MAG[0]; - H_MAG[15] = 2*q0*q2 + 2*q1*q3; - H_MAG[16] = 2*q2*q3 - 2*q0*q1; - H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - H_MAG[20] = 1; - - // Calculate Kalman gain - SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; - SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MZ[4] = 2*q0*q1 - 2*q2*q3; - SK_MZ[5] = 2*q0*q2 + 2*q1*q3; - Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); - Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); - Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); - Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); - Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); - Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); - Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); - Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); - Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); - Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); - Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); - Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); - Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); - Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); - Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); - Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); - Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); - Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); - Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); - Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); - Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); - varInnovMag[2] = 1.0f/SK_MZ[0]; - innovMag[2] = MagPred[2] - magData.z; - - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) - { - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=3; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; - if (!onGround) - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - } - else - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = 0.0f; - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k<=3; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - if (!onGround) - { - for (uint8_t k = 15; k<=20; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::FuseAirspeed() -{ - float vn; - float ve; - float vd; - float vwn; - float vwe; - const float R_TAS = 2.0f; - float SH_TAS[3]; - float Kfusion[21]; - float VtasPred; - - // Copy required states to local variable names - vn = statesAtVtasMeasTime[4]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[13]; - vwe = statesAtVtasMeasTime[14]; - - // Need to check that it is flying before fusing airspeed data - // Calculate the predicted airspeed - VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); - // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) - { - // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - - float H_TAS[21]; - for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; - H_TAS[4] = SH_TAS[2]; - H_TAS[5] = SH_TAS[1]; - H_TAS[6] = vd*SH_TAS[0]; - H_TAS[13] = -SH_TAS[2]; - H_TAS[14] = -SH_TAS[1]; - - // Calculate Kalman gains - float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - varInnovVtas = 1.0f/SK_TAS; - - // Calculate the measurement innovation - innovVtas = VtasPred - VtasMeas; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) - { - // correct the state vector - for (uint8_t j=0; j<=20; j++) - { - states[j] = states[j] - Kfusion[j] * innovVtas; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; - for (uint8_t j = 13; j<=14; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 13; k<=14; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col<n_states; col++) - { - covMat[row][col] = 0.0; - } - } -} - -void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (col=first; col<=last; col++) - { - for (row=0; row < n_states; row++) - { - covMat[row][col] = 0.0; - } - } -} - -float AttPosEKF::sq(float valIn) -{ - return valIn*valIn; -} - -// Store states in a history array along with time stamp -void AttPosEKF::StoreStates(uint64_t timestamp_ms) -{ - for (unsigned i=0; i<n_states; i++) - storedStates[i][storeIndex] = states[i]; - statetimeStamp[storeIndex] = timestamp_ms; - storeIndex++; - if (storeIndex == data_buffer_size) - storeIndex = 0; -} - -void AttPosEKF::ResetStoredStates() -{ - // reset all stored states - memset(&storedStates[0][0], 0, sizeof(storedStates)); - memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); - - // reset store index to first - storeIndex = 0; - - // overwrite all existing states - for (unsigned i = 0; i < n_states; i++) { - storedStates[i][storeIndex] = states[i]; - } - - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; -} - -// Output the state vector stored at the time that best matches that specified by msec -int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) -{ - int ret = 0; - - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // The time delta can also end up as negative number, - // // since we might compare future to past or past to future - // // therefore cast to int64. - // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - // if (timeDelta < 0) { - // timeDelta = -timeDelta; - // } - - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) { - // if (isfinite(storedStates[i][bestStoreIndex])) { - // statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } else if (isfinite(states[i])) { - // statesForFusion[i] = states[i]; - // } else { - // // There is not much we can do here, except reporting the error we just - // // found. - // ret++; - // } - // } - // else // otherwise output current state - // { - for (uint8_t i=0; i < n_states; i++) { - if (isfinite(states[i])) { - statesForFusion[i] = states[i]; - } else { - ret++; - } - } - // } - - return ret; -} - -void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) -{ - // Calculate the nav to body cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tnb.x.x = q00 + q11 - q22 - q33; - Tnb.y.y = q00 - q11 + q22 - q33; - Tnb.z.z = q00 - q11 - q22 + q33; - Tnb.y.x = 2*(q12 - q03); - Tnb.z.x = 2*(q13 + q02); - Tnb.x.y = 2*(q12 + q03); - Tnb.z.y = 2*(q23 - q01); - Tnb.x.z = 2*(q13 - q02); - Tnb.y.z = 2*(q23 + q01); -} - -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) -{ - // Calculate the body to nav cosine matrix - float q00 = sq(quat[0]); - float q11 = sq(quat[1]); - float q22 = sq(quat[2]); - float q33 = sq(quat[3]); - float q01 = quat[0]*quat[1]; - float q02 = quat[0]*quat[2]; - float q03 = quat[0]*quat[3]; - float q12 = quat[1]*quat[2]; - float q13 = quat[1]*quat[3]; - float q23 = quat[2]*quat[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); -} - -void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) -{ - float u1 = cos(0.5f*eul[0]); - float u2 = cos(0.5f*eul[1]); - float u3 = cos(0.5f*eul[2]); - float u4 = sin(0.5f*eul[0]); - float u5 = sin(0.5f*eul[1]); - float u6 = sin(0.5f*eul[2]); - quat[0] = u1*u2*u3+u4*u5*u6; - quat[1] = u4*u2*u3-u1*u5*u6; - quat[2] = u1*u5*u3+u4*u2*u6; - quat[3] = u1*u2*u6-u4*u5*u3; -} - -void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) -{ - y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); - y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); - y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); -} - -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) -{ - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); -} - -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) -{ - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - -void AttPosEKF::OnGroundCheck() -{ - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); - if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); - } -} - -void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) -{ - //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cosf(latitude); - omega.y = 0.0f; - omega.z = -earthRate*sinf(latitude); -} - -void AttPosEKF::CovarianceInit() -{ - // Calculate the initial covariance matrix P - P[0][0] = 0.25f * sq(1.0f*deg2rad); - P[1][1] = 0.25f * sq(1.0f*deg2rad); - P[2][2] = 0.25f * sq(1.0f*deg2rad); - P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7); - P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); - P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); - P[11][11] = P[10][10]; - P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; -} - -float AttPosEKF::ConstrainFloat(float val, float min, float max) -{ - return (val > max) ? max : ((val < min) ? min : val); -} - -void AttPosEKF::ConstrainVariances() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Constrain position variances - for (unsigned i = 7; i < 10; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); - } - - // Angle bias variances - for (unsigned i = 10; i < 13; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); - } - - // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Earth magnetic field variances - for (unsigned i = 15; i < 18; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Body magnetic field variances - for (unsigned i = 18; i < 21; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - -} - -void AttPosEKF::ConstrainStates() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - - // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i < 7; i++) { - states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); - } - - // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i < 9; i++) { - states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); - } - - // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); - - // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); - } - - // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { - states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); - } - - // Earth magnetic field limits (in Gauss) - for (unsigned i = 15; i < 18; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Body magnetic field variances (in Gauss). - // the max offset should be in this range. - for (unsigned i = 18; i < 21; i++) { - states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); - } - -} - -void AttPosEKF::ForceSymmetry() -{ - if (!numericalProtection) { - return; - } - - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (P[i][j] + P[j][i]); - P[j][i] = P[i][j]; - } - } -} - -bool AttPosEKF::FilterHealthy() -{ - if (!statesInitialised) { - return false; - } - - // XXX Check state vector for NaNs and ill-conditioning - - // Check if any of the major inputs timed out - if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { - return false; - } - - // Nothing fired, return ok. - return true; -} - -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ -void AttPosEKF::ResetPosition(void) -{ - if (staticMode) { - states[7] = 0; - states[8] = 0; - } else if (GPSstatus >= GPS_FIX_3D) { - - // reset the states from the GPS measurements - states[7] = posNE[0]; - states[8] = posNE[1]; - } -} - -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ -void AttPosEKF::ResetHeight(void) -{ - // write to the state vector - states[9] = -hgtMea; -} - -/** - * Reset the velocity state. - */ -void AttPosEKF::ResetVelocity(void) -{ - if (staticMode) { - states[4] = 0.0f; - states[5] = 0.0f; - states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { - - states[4] = velNED[0]; // north velocity from last reading - states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading - } -} - - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (int i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { - bool err = false; - - // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - if (!isfinite(KH[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // covariance matrix - } - - if (!isfinite(Kfusion[i])) { - - err_report->kalmanGainsNaN = true; - err = true; - } // Kalman gains - - if (!isfinite(states[i])) { - - err_report->statesNaN = true; - err = true; - } // state matrix - } - - if (err) { - FillErrorReport(err_report); - } - - return err; - -} - -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ -int AttPosEKF::CheckAndBound() -{ - - // Store the old filter state - bool currStaticMode = staticMode; - - // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { - - InitializeDynamic(velNED, magDeclination); - - return 1; - } - - // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { - - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - // that's all we can do here, return - return 2; - } - - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - - // Check if we switched between states - if (currStaticMode != staticMode) { - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - return 3; - } - - return 0; -} - -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - /* true heading is the mag heading minus declination */ - initialHdg += declination; - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; -} - -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) -{ - - // Clear the init flag - statesInitialised = false; - - magDeclination = declination; - - ZeroVariables(); - - // Calculate initial filter quaternion states from raw measurements - float initQuat[4]; - Vector3f initMagXYZ; - initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); - - // Calculate initial Tbn matrix and rotate Mag measurements into NED - // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); - Vector3f initMagNED; - initMagXYZ = magData - magBias; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - - - - // write to state vector - for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z - - statesInitialised = true; - - // initialise the covariance matrix - CovarianceInit(); - - //Define Earth rotation vector in the NED navigation frame - calcEarthRateNED(earthRateNED, latRef); - - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; -} - -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) -{ - //store initial lat,long and height - latRef = referenceLat; - lonRef = referenceLon; - hgtRef = referenceHgt; - - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - - InitializeDynamic(initvelNED, declination); -} - -void AttPosEKF::ZeroVariables() -{ - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } - - memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); -} - -void AttPosEKF::GetFilterState(struct ekf_status_report *state) -{ - memcpy(state, ¤t_ekf_state, sizeof(state)); -} - -void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) -{ - memcpy(last_error, &last_ekf_error, sizeof(last_error)); -} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h deleted file mode 100644 index a19ff1995..000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.h +++ /dev/null @@ -1,247 +0,0 @@ -#pragma once - -#include "estimator_utilities.h" - -class AttPosEKF { - -public: - - AttPosEKF(); - ~AttPosEKF(); - - /* ############################################## - * - * M A I N F I L T E R P A R A M E T E R S - * - * ########################################### */ - - /* - * parameters are defined here and initialised in - * the InitialiseParameters() (which is just 20 lines down) - */ - - float covTimeStepMax; // maximum time allowed between covariance predictions - float covDelAngMax; // maximum delta angle between covariance predictions - float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - - float yawVarScale; - float windVelSigma; - float dAngBiasSigma; - float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float gndHgtSigma; - - float vneSigma; - float vdSigma; - float posNeSigma; - float posDSigma; - float magMeasurementSigma; - float airspeedMeasurementSigma; - - float gyroProcessNoise; - float accelProcessNoise; - - float EAS2TAS; // ratio f true to equivalent airspeed - - void InitialiseParameters() - { - covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions - covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - EAS2TAS = 1.0f; - - yawVarScale = 1.0f; - windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma - - vneSigma = 0.2f; - vdSigma = 0.3f; - posNeSigma = 2.0f; - posDSigma = 2.0f; - - magMeasurementSigma = 0.05; - airspeedMeasurementSigma = 1.4f; - gyroProcessNoise = 1.4544411e-2f; - accelProcessNoise = 0.5f; - } - - // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored - - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - - Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) - Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) - Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) - Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) - Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) - Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) - Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) - Vector3f dVelIMU; - Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity - float innovVelPos[6]; // innovation output - float varInnovVelPos[6]; // innovation variance output - - float velNED[3]; // North, East, Down velocity obs (m/s) - float posNE[2]; // North, East position obs (m) - float hgtMea; // measured height (m) - float posNED[3]; // North, East Down position (m) - - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output - Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output - float varInnovVtas; // innovation variance output - float VtasMeas; // true airspeed measurement (m/s) - float magDeclination; - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) - float hgtRef; // WGS-84 height of reference point (m) - Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - - // GPS input data variables - float gpsCourse; - float gpsVelD; - float gpsLat; - float gpsLon; - float gpsHgt; - uint8_t GPSstatus; - - // Baro input - float baroHgt; - - bool statesInitialised; - - bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData; // this boolean causes the hgtMea obs to be fused - bool fuseMagData; // boolean true when magnetometer data is to be fused - bool fuseVtasData; // boolean true when airspeed data is to be fused - - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode; ///< boolean true if no position feedback is fused - bool useAirspeed; ///< boolean true if airspeed data is being used - bool useCompass; ///< boolean true if magnetometer data is being used - - struct ekf_status_report current_ekf_state; - struct ekf_status_report last_ekf_error; - - bool numericalProtection; - - unsigned storeIndex; - - -void UpdateStrapdownEquationsNED(); - -void CovariancePrediction(float dt); - -void FuseVelposNED(); - -void FuseMagnetometer(); - -void FuseAirspeed(); - -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void quatNorm(float (&quatOut)[4], const float quatIn[4]); - -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); - -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - * - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); - -void ResetStoredStates(); - -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); - -void calcEarthRateNED(Vector3f &omega, float latitude); - -static void eul2quat(float (&quat)[4], const float (&eul)[3]); - -static void quat2eul(float (&eul)[3], const float (&quat)[4]); - -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); - -static float sq(float valIn); - -void OnGroundCheck(); - -void CovarianceInit(); - -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); - -float ConstrainFloat(float val, float min, float max); - -void ConstrainVariances(); - -void ConstrainStates(); - -void ForceSymmetry(); - -int CheckAndBound(); - -void ResetPosition(); - -void ResetVelocity(); - -void ZeroVariables(); - -void GetFilterState(struct ekf_status_report *state); - -void GetLastErrorState(struct ekf_status_report *last_error); - -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); - -void InitializeDynamic(float (&initvelNED)[3], float declination); - -protected: - -bool FilterHealthy(); - -void ResetHeight(void); - -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); - -}; - -uint32_t millis(); - diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index c17e034ad..15d018ab6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,4 +1,4 @@ -#include "estimator_23states.h" +#include "estimator_22states.h" #include <string.h> #include <stdio.h> #include <stdarg.h> @@ -14,9 +14,6 @@ AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), covDelAngMax(0.0f), rngFinderPitch(0.0f), - a1(0.0f), - a2(0.0f), - a3(0.0f), yawVarScale(0.0f), windVelSigma(0.0f), dAngBiasSigma(0.0f), @@ -71,10 +68,6 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), - windSpdFiltNorth(0.0f), - windSpdFiltEast(0.0f), - windSpdFiltAltitude(0.0f), - windSpdFiltClimb(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -87,7 +80,6 @@ AttPosEKF::AttPosEKF() : innovMag{}, varInnovMag{}, magData{}, - losData{}, innovVtas(0.0f), innovRng(0.0f), innovOptFlow{}, @@ -102,6 +94,8 @@ AttPosEKF::AttPosEKF() : refSet(false), magBias(), covSkipCount(0), + lastFixTime_ms(0), + globalTimeStamp_ms(0), gpsLat(0.0), gpsLon(-M_PI), gpsHgt(0.0f), @@ -123,6 +117,7 @@ AttPosEKF::AttPosEKF() : onGround(true), staticMode(true), + useGPS(false), useAirspeed(true), useCompass(true), useRangeFinder(true), @@ -133,7 +128,7 @@ AttPosEKF::AttPosEKF() : current_ekf_state{}, last_ekf_error{}, numericalProtection(true), - storeIndex(0), + storeIndex(0), storedOmega{}, Popt{}, flowStates{}, @@ -152,6 +147,7 @@ AttPosEKF::AttPosEKF() : minFlowRng(0.0f), moCompR_LOS(0.0f) { + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); @@ -185,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z - states[13]; + + Vector3f dVelIMURel; + + dVelIMURel.x = dVelIMU.x; + dVelIMURel.y = dVelIMU.y; + dVelIMURel.z = dVelIMU.z - states[13]; delAngTotal.x += correctedDelAng.x; delAngTotal.y += correctedDelAng.y; @@ -267,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; // calculate the magnitude of the nav acceleration (required for GPS // variance estimation) @@ -344,14 +343,9 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; - } else { - for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; - } - if (!inhibitGndState) { - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; } else { - processNoise[22] = 0; + for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; } // square all sigmas @@ -451,7 +445,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; - nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; @@ -474,7 +467,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; - nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; @@ -497,7 +489,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; - nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; @@ -520,7 +511,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; - nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; @@ -543,7 +533,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; - nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; @@ -566,7 +555,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; - nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; @@ -589,7 +577,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; - nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; @@ -612,7 +599,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[7][20] = P[7][20] + P[4][20]*dt; nextP[7][21] = P[7][21] + P[4][21]*dt; - nextP[7][22] = P[7][22] + P[4][22]*dt; nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; @@ -635,7 +621,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[8][19] = P[8][19] + P[5][19]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; nextP[8][21] = P[8][21] + P[5][21]*dt; - nextP[8][22] = P[8][22] + P[5][22]*dt; nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; @@ -658,7 +643,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[9][21] = P[9][21] + P[6][21]*dt; - nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; @@ -681,7 +665,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[10][19] = P[10][19]; nextP[10][20] = P[10][20]; nextP[10][21] = P[10][21]; - nextP[10][22] = P[10][22]; nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; @@ -704,7 +687,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[11][19] = P[11][19]; nextP[11][20] = P[11][20]; nextP[11][21] = P[11][21]; - nextP[11][22] = P[11][22]; nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; @@ -727,7 +709,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[12][19] = P[12][19]; nextP[12][20] = P[12][20]; nextP[12][21] = P[12][21]; - nextP[12][22] = P[12][22]; nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; @@ -750,7 +731,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[13][19] = P[13][19]; nextP[13][20] = P[13][20]; nextP[13][21] = P[13][21]; - nextP[13][22] = P[13][22]; nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; @@ -773,7 +753,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[14][19] = P[14][19]; nextP[14][20] = P[14][20]; nextP[14][21] = P[14][21]; - nextP[14][22] = P[14][22]; nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; @@ -796,7 +775,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[15][19] = P[15][19]; nextP[15][20] = P[15][20]; nextP[15][21] = P[15][21]; - nextP[15][22] = P[15][22]; nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; @@ -819,7 +797,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[16][19] = P[16][19]; nextP[16][20] = P[16][20]; nextP[16][21] = P[16][21]; - nextP[16][22] = P[16][22]; nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; @@ -842,7 +819,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[17][19] = P[17][19]; nextP[17][20] = P[17][20]; nextP[17][21] = P[17][21]; - nextP[17][22] = P[17][22]; nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; @@ -865,7 +841,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[18][19] = P[18][19]; nextP[18][20] = P[18][20]; nextP[18][21] = P[18][21]; - nextP[18][22] = P[18][22]; nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; @@ -888,7 +863,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[19][19] = P[19][19]; nextP[19][20] = P[19][20]; nextP[19][21] = P[19][21]; - nextP[19][22] = P[19][22]; nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; @@ -911,7 +885,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; nextP[20][21] = P[20][21]; - nextP[20][22] = P[20][22]; nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; @@ -934,30 +907,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][19] = P[21][19]; nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - nextP[21][22] = P[21][22]; - nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; - nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; - nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; - nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; - nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; - nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; - nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; - nextP[22][7] = P[22][7] + P[22][4]*dt; - nextP[22][8] = P[22][8] + P[22][5]*dt; - nextP[22][9] = P[22][9] + P[22][6]*dt; - nextP[22][10] = P[22][10]; - nextP[22][11] = P[22][11]; - nextP[22][12] = P[22][12]; - nextP[22][13] = P[22][13]; - nextP[22][14] = P[22][14]; - nextP[22][15] = P[22][15]; - nextP[22][16] = P[22][16]; - nextP[22][17] = P[22][17]; - nextP[22][18] = P[22][18]; - nextP[22][19] = P[22][19]; - nextP[22][20] = P[22][20]; - nextP[22][21] = P[22][21]; - nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -1031,7 +980,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit = 22; + uint8_t indexLimit = 21; // declare variables used by state and covariance update calculations float velErr; @@ -1230,15 +1179,11 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i<=21; i++) + for (uint8_t i = 16; i < n_states; i++) { Kfusion[i] = 0; } } - // Don't update terrain state if inhibited - if (inhibitGndState) { - Kfusion[22] = 0; - } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1415,15 +1360,10 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } - if (!inhibitGndState) { - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1598,14 +1538,14 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = 0.0f; } @@ -1622,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k<=21; k++) + for (uint8_t k = 16; k < n_states; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1669,32 +1609,6 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { - - float altDiff = fabsf(windSpdFiltAltitude - hgtMea); - - if (isfinite(windSpdFiltClimb)) { - windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]); - } else { - windSpdFiltClimb = states[6]; - } - - if (altDiff < 20.0f) { - // Lowpass the output of the wind estimate - we want a long-term - // stable estimate, but not start to load into the overall dynamics - // of the system (which adjusting covariances would do) - - // Change filter coefficient based on altitude change rate - float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f); - - windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); - windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); - } else { - windSpdFiltNorth = vwn; - windSpdFiltEast = vwe; - } - - windSpdFiltAltitude = hgtMea; - // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; @@ -1750,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1762,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j <= 22; j++) + for (uint8_t j=0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1779,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1791,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1808,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1835,356 +1749,269 @@ void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uin } } -void AttPosEKF::FuseRangeFinder() +void AttPosEKF::FuseOptFlow() { - - // Local variables - float rngPred; - float SH_RNG[5]; - float H_RNG[23]; - float SK_RNG[6]; - float cosRngTilt; - const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance - - // Copy required states to local variable names - float q0 = statesAtRngTime[0]; - float q1 = statesAtRngTime[1]; - float q2 = statesAtRngTime[2]; - float q3 = statesAtRngTime[3]; - float pd = statesAtRngTime[9]; - float ptd = statesAtRngTime[22]; - - // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sinf(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) + static float SH_LOS[13]; + static float SK_LOS[9]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float losPred[2]; + + // Transformation matrix from nav to body axes + float H_LOS[2][n_states]; + float K_LOS[2][n_states]; + Vector3f velNED_local; + Vector3f relVelSensor; + + // Perform sequential fusion of optical flow measurements only with valid tilt and height + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; + bool validTilt = Tnb.z.z > 0.71f; + if (validTilt) { - // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset - // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations - SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_RNG[1] = pd - ptd; - SH_RNG[2] = 1/sq(SH_RNG[0]); - SH_RNG[3] = 1/SH_RNG[0]; - for (uint8_t i = 0; i < n_states; i++) { - H_RNG[i] = 0.0f; - Kfusion[i] = 0.0f; - } - H_RNG[22] = -SH_RNG[3]; - SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); - SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; - SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; - SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; - SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; - SK_RNG[5] = SH_RNG[2]; - Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + // Sequential fusion of XY components. - // Calculate the innovation variance for data logging - varInnovRng = 1.0f/SK_RNG[0]; + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vn = statesAtFlowTime[4]; + ve = statesAtFlowTime[5]; + vd = statesAtFlowTime[6]; + pd = statesAtFlowTime[9]; + ptd = flowStates[1]; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; + + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = heightAboveGndEst/Tnb_flow.z.z; + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow*velNED_local; + + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // Calculate common expressions for observation jacobians + SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); + SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 1/sq(pd - ptd); + + // Calculate common expressions for Kalman gains + SK_LOS[0] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[0] * moCompR_LOS)) + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)))); + SK_LOS[1] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[1] * moCompR_LOS))+ (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)))); + SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn); + SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn); + SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SK_LOS[8] = SH_LOS[3]; + + // Calculate common intermediate terms + float tempVar[9]; + tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8]; + tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4]; + tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8]; + tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3); + tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2); + tempVar[5] = (SK_LOS[5] - q2*tempVar[2]); + tempVar[6] = (SK_LOS[2] - q3*tempVar[2]); + tempVar[7] = (SK_LOS[3] - q1*tempVar[2]); + tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); + + // calculate observation jacobians for X LOS rate + for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0; + H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + H_LOS[0][3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2); + H_LOS[0][5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + H_LOS[0][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3); + H_LOS[0][9] = tempVar[1]; + + // calculate Kalman gains for X LOS rate + K_LOS[0][0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0])); + K_LOS[0][1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]); + K_LOS[0][2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]); + K_LOS[0][3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]); + K_LOS[0][4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]); + K_LOS[0][5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]); + K_LOS[0][6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]); + K_LOS[0][7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]); + K_LOS[0][8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]); + K_LOS[0][9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]); + K_LOS[0][10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]); + K_LOS[0][11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]); + K_LOS[0][12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[0][13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]); + if (inhibitWindStates) { + K_LOS[0][14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]); + K_LOS[0][15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]); + } else { + K_LOS[0][14] = 0.0f; + K_LOS[0][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[0][16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]); + K_LOS[0][17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]); + K_LOS[0][18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]); + K_LOS[0][19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]); + K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); + K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); + } else { + for (uint8_t i = 16; i < n_states; i++) { + K_LOS[0][i] = 0.0f; + } + } - // Calculate the measurement innovation - rngPred = (ptd - pd)/cosRngTilt; - innovRng = rngPred - rngMea; + // calculate innovation variance and innovation for X axis observation + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - flowRadXYcomp[0]; + + // calculate intermediate common variables + tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8]; + tempVar[1] = (SK_LOS[2] + q0*tempVar[0]); + tempVar[2] = (SK_LOS[5] - q1*tempVar[0]); + tempVar[3] = (SK_LOS[3] + q2*tempVar[0]); + tempVar[4] = (SK_LOS[4] + q3*tempVar[0]); + tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2); + tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3); + tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4]; + tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; + + // Calculate observation jacobians for Y LOS rate + for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0; + H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); + H_LOS[1][5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2); + H_LOS[1][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3); + H_LOS[1][9] = -tempVar[7]; + + // Calculate Kalman gains for Y LOS rate + K_LOS[1][0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]); + K_LOS[1][1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]); + K_LOS[1][2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]); + K_LOS[1][3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]); + K_LOS[1][4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]); + K_LOS[1][5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]); + K_LOS[1][6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]); + K_LOS[1][7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]); + K_LOS[1][8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]); + K_LOS[1][9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]); + K_LOS[1][10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]); + K_LOS[1][11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]); + K_LOS[1][12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[1][13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]); + if (inhibitWindStates) { + K_LOS[1][14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]); + K_LOS[1][15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]); + } else { + K_LOS[1][14] = 0.0f; + K_LOS[1][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); + K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); + K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); + K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); + K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); + K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); + } else { + for (uint8_t i = 16; i < n_states; i++) { + K_LOS[1][i] = 0.0f; + } + } - // calculate the innovation consistency test ratio - auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); + // calculate variance and innovation for Y observation + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - flowRadXYcomp[1]; - // Check the innovation for consistency and don't fuse if out of bounds - if (auxRngTestRatio < 1.0f) - { - // correct the state vector - states[22] = states[22] - Kfusion[22] * innovRng; + } - // correct the covariance P = (I - K*H)*P - P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + // loop through the X and Y observations and fuse them sequentially + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; + for (uint8_t j = 10; j < n_states; j++) + { + KH[i][j] = 0.0f; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } } + ForceSymmetry(); + ConstrainVariances(); } - -} - -void AttPosEKF::FuseOptFlow() -{ -// static uint8_t obsIndex; -// static float SH_LOS[13]; -// static float SKK_LOS[15]; -// static float SK_LOS[2]; -// static float q0 = 0.0f; -// static float q1 = 0.0f; -// static float q2 = 0.0f; -// static float q3 = 1.0f; -// static float vn = 0.0f; -// static float ve = 0.0f; -// static float vd = 0.0f; -// static float pd = 0.0f; -// static float ptd = 0.0f; -// static float R_LOS = 0.01f; -// static float losPred[2]; - -// // Transformation matrix from nav to body axes -// Mat3f Tnb_local; -// // Transformation matrix from body to sensor axes -// // assume camera is aligned with Z body axis plus a misalignment -// // defined by 3 small angles about X, Y and Z body axis -// Mat3f Tbs; -// Tbs.x.y = a3; -// Tbs.y.x = -a3; -// Tbs.x.z = -a2; -// Tbs.z.x = a2; -// Tbs.y.z = a1; -// Tbs.z.y = -a1; -// // Transformation matrix from navigation to sensor axes -// Mat3f Tns; -// float H_LOS[n_states]; -// for (uint8_t i = 0; i < n_states; i++) { -// H_LOS[i] = 0.0f; -// } -// Vector3f velNED_local; -// Vector3f relVelSensor; - -// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. -// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) -// { -// // Sequential fusion of XY components to spread processing load across -// // two prediction time steps. - -// // Calculate observation jacobians and Kalman gains -// if (fuseOptFlowData) -// { -// // Copy required states to local variable names -// q0 = statesAtOptFlowTime[0]; -// q1 = statesAtOptFlowTime[1]; -// q2 = statesAtOptFlowTime[2]; -// q3 = statesAtOptFlowTime[3]; -// vn = statesAtOptFlowTime[4]; -// ve = statesAtOptFlowTime[5]; -// vd = statesAtOptFlowTime[6]; -// pd = statesAtOptFlowTime[9]; -// ptd = statesAtOptFlowTime[22]; -// velNED_local.x = vn; -// velNED_local.y = ve; -// velNED_local.z = vd; - -// // calculate rotation from NED to body axes -// float q00 = sq(q0); -// float q11 = sq(q1); -// float q22 = sq(q2); -// float q33 = sq(q3); -// float q01 = q0 * q1; -// float q02 = q0 * q2; -// float q03 = q0 * q3; -// float q12 = q1 * q2; -// float q13 = q1 * q3; -// float q23 = q2 * q3; -// Tnb_local.x.x = q00 + q11 - q22 - q33; -// Tnb_local.y.y = q00 - q11 + q22 - q33; -// Tnb_local.z.z = q00 - q11 - q22 + q33; -// Tnb_local.y.x = 2*(q12 - q03); -// Tnb_local.z.x = 2*(q13 + q02); -// Tnb_local.x.y = 2*(q12 + q03); -// Tnb_local.z.y = 2*(q23 - q01); -// Tnb_local.x.z = 2*(q13 - q02); -// Tnb_local.y.z = 2*(q23 + q01); - -// // calculate transformation from NED to sensor axes -// Tns = Tbs*Tnb_local; - -// // calculate range from ground plain to centre of sensor fov assuming flat earth -// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); - -// // calculate relative velocity in sensor frame -// relVelSensor = Tns*velNED_local; - -// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes -// losPred[0] = relVelSensor.y/range; -// losPred[1] = -relVelSensor.x/range; - -// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); -// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); -// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); - -// // Calculate observation jacobians -// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); -// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// SH_LOS[3] = 1/(pd - ptd); -// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; -// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; -// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; -// SH_LOS[7] = 1/sq(pd - ptd); -// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; -// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; -// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; -// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; -// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); -// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); -// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; -// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - -// // Calculate Kalman gain -// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); -// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); -// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); -// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); -// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); -// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); -// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); -// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); -// SKK_LOS[14] = SH_LOS[0]; - -// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); -// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// varInnovOptFlow[0] = 1.0f/SK_LOS[0]; -// innovOptFlow[0] = losPred[0] - losData[0]; - -// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag -// obsIndex = 1; -// fuseOptFlowData = false; -// } -// else if (obsIndex == 1) // we are now fusing the Y measurement -// { -// // Calculate observation jacobians -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); -// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); -// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; -// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - -// // Calculate Kalman gains -// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); -// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// varInnovOptFlow[1] = 1.0f/SK_LOS[1]; -// innovOptFlow[1] = losPred[1] - losData[1]; - -// // reset the observation index -// obsIndex = 0; -// fuseOptFlowData = false; -// } - -// // Check the innovation for consistency and don't fuse if > 3Sigma -// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) -// { -// // correct the state vector -// for (uint8_t j = 0; j < n_states; j++) -// { -// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; -// } -// // normalise the quaternion states -// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); -// if (quatMag > 1e-12f) -// { -// for (uint8_t j= 0; j<=3; j++) -// { -// float quatMagInv = 1.0f/quatMag; -// states[j] = states[j] * quatMagInv; -// } -// } -// // correct the covariance P = (I - K*H)*P -// // take advantage of the empty columns in KH to reduce the -// // number of operations -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j <= 6; j++) -// { -// KH[i][j] = Kfusion[i] * H_LOS[j]; -// } -// for (uint8_t j = 7; j <= 8; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][9] = Kfusion[i] * H_LOS[9]; -// for (uint8_t j = 10; j <= 21; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][22] = Kfusion[i] * H_LOS[22]; -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// KHP[i][j] = 0.0f; -// for (uint8_t k = 0; k <= 6; k++) -// { -// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; -// } -// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; -// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; -// } -// } -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// P[i][j] = P[i][j] - KHP[i][j]; -// } -// } -// ForceSymmetry(); -// ConstrainVariances(); -// } } /* @@ -2192,27 +2019,29 @@ Estimation of optical flow sensor focal length scale factor and terrain height u This fiter requires optical flow rates that are not motion compensated Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser */ -void AttPosEKF::GroundEKF() +void AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning if (!inhibitGndState) { float distanceTravelledSq; - distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); - prevPosN = statesAtRngTime[7]; - prevPosE = statesAtRngTime[8]; + if (fuseRngData) { + distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); + prevPosN = statesAtRngTime[7]; + prevPosE = statesAtRngTime[8]; + } else if (fuseOptFlowData) { + distanceTravelledSq = sq(statesAtFlowTime[7] - prevPosN) + sq(statesAtFlowTime[8] - prevPosE); + prevPosN = statesAtFlowTime[7]; + prevPosE = statesAtFlowTime[8]; + } else { + return; + } distanceTravelledSq = min(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } - // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems - Popt[0][0] = 0.0f; - Popt[0][1] = 0.0f; - Popt[1][0] = 0.0f; - // Fuse range finder data - // Need to check that our range finder tilt angle is less than 30 degrees - float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { + // fuse range finder data + if (fuseRngData) { float range; // range from camera to centre of image float q0; // quaternion at optical flow measurement time float q1; // quaternion at optical flow measurement time @@ -2266,10 +2095,7 @@ void AttPosEKF::GroundEKF() flowStates[i] -= K_RNG[i] * innovRng; } // constrain the states - - // constrain focal length to 0.1 to 10 mm flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - // constrain altitude flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix @@ -2285,6 +2111,132 @@ void AttPosEKF::GroundEKF() Popt[1][0] = Popt[0][1]; } } + + if (fuseOptFlowData) { + Vector3f vel; // velocity of sensor relative to ground in NED axes + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + float losPred[2]; // predicted optical flow angular rate measurements + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float HP[2]; + float SH_OPT[6]; + float SK_OPT[3]; + float K_OPT[2][2]; + float H_OPT[2][2]; + float nextPopt[2][2]; + + // propagate scale factor state noise + if (!inhibitScaleState) { + Popt[0][0] += 1e-8f; + } else { + Popt[0][0] = 0.0f; + } + + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vel.x = statesAtFlowTime[4]; + vel.y = statesAtFlowTime[5]; + vel.z = statesAtFlowTime[6]; + + // constrain terrain height to be below the vehicle + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // estimate range to centre of image + range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow * vel; + + // divide velocity by range, subtract body rates and apply scale factor to + // get predicted sensed angular optical rates relative to X and Y sensor axes + losPred[0] = flowStates[0]*( relVelSensor.y / range) - omegaAcrossFlowTime[0]; + losPred[1] = flowStates[0]*(-relVelSensor.x / range) - omegaAcrossFlowTime[1]; + + // calculate innovations + auxFlowObsInnov[0] = losPred[0] - flowRadXY[0]; + auxFlowObsInnov[1] = losPred[1] - flowRadXY[1]; + + // calculate Kalman gains + SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3); + SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3); + SH_OPT[3] = statesAtFlowTime[9] - flowStates[1]; + SH_OPT[4] = 1.0f/sq(SH_OPT[3]); + SH_OPT[5] = 1.0f/SH_OPT[3]; + float SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5]; + float SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5]; + float SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4]; + float SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4]; + SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014)); + SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024)); + SK_OPT[2] = SH_OPT[0]; + if (!inhibitScaleState) { + K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[0][0] = 0.0f; + K_OPT[0][1] = 0.0f; + } + if (!inhibitGndState) { + K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[1][0] = 0.0f; + K_OPT[1][1] = 0.0f; + } + + // calculate innovation variances + auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1]; + auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0]; + + // calculate observations jacobians + H_OPT[0][0] = -SH025; + H_OPT[0][1] = -flowStates[0]*SH024; + H_OPT[1][0] = SH015; + H_OPT[1][1] = flowStates[0]*SH014; + + // Check the innovation for consistency and don't fuse if > threshold + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + + // calculate the innovation consistency test ratio + auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(auxFlowInnovGate) * auxFlowObsInnovVar[obsIndex]); + if (auxFlowTestRatio[obsIndex] < 1.0f) { + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; + } + // constrain the states + flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // correct the covariance matrix + for (uint8_t i = 0; i < 2 ; i++) { + HP[i] = 0.0f; + for (uint8_t j = 0; j < 2 ; j++) { + HP[i] += H_OPT[obsIndex][j] * P[j][i]; + } + } + for (uint8_t i = 0; i < 2 ; i++) { + for (uint8_t j = 0; j < 2 ; j++) { + nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j]; + } + } + + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = maxf(nextPopt[0][0],0.0f); + Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = Popt[0][1]; + } + } + } + } void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -2328,6 +2280,9 @@ void AttPosEKF::StoreStates(uint64_t timestamp_ms) { for (unsigned i=0; i<n_states; i++) storedStates[i][storeIndex] = states[i]; + storedOmega[0][storeIndex] = angRate.x; + storedOmega[1][storeIndex] = angRate.y; + storedOmega[2][storeIndex] = angRate.z; statetimeStamp[storeIndex] = timestamp_ms; storeIndex++; if (storeIndex == data_buffer_size) @@ -2338,16 +2293,12 @@ void AttPosEKF::ResetStoredStates() { // reset all stored states memset(&storedStates[0][0], 0, sizeof(storedStates)); + memset(&storedOmega[0][0], 0, sizeof(storedOmega)); memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); // reset store index to first storeIndex = 0; - // overwrite all existing states - for (unsigned i = 0; i < n_states; i++) { - storedStates[i][storeIndex] = states[i]; - } - statetimeStamp[storeIndex] = millis(); // increment to next storage index @@ -2407,6 +2358,37 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) return ret; } +void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec) +{ + // work back in time and calculate average angular rate over the time interval + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] = 0.0f; + } + uint8_t sumIndex = 0; + int64_t timeDelta; + for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) + { + // calculate the average of all samples younger than msec + timeDelta = statetimeStamp[storeIndexLocal] - msec; + if (timeDelta > 0) + { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] += storedOmega[i][storeIndexLocal]; + } + sumIndex += 1; + } + } + if (sumIndex >= 1) { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); + } + } else { + omegaForFusion[0] = angRate.x; + omegaForFusion[1] = angRate.y; + omegaForFusion[2] = angRate.z; + } +} + void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2502,6 +2484,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2517,12 +2500,25 @@ void AttPosEKF::OnGroundCheck() } else { inhibitMagStates = false; } - // don't update terrain offset state if on ground - if (onGround) { + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS + if ((onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable + if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + inhibitGndState = true; + } else { + inhibitGndState = false; + } + // Don't update focal length offset state if there is no range finder or optical flow sensor + // we need both sensors to do this estimation + if (!useRangeFinder || !useOpticalFlow) { + inhibitScaleState = true; + } else { + inhibitScaleState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -2558,7 +2554,9 @@ void AttPosEKF::CovarianceInit() P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; - P[22][22] = sq(0.5f); + + fScaleFactorVar = 0.001f; // focal length scale factor variance + Popt[0][0] = 0.001f; } float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) @@ -2596,7 +2594,6 @@ void AttPosEKF::ConstrainVariances() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion variances for (unsigned i = 0; i <= 3; i++) { @@ -2636,8 +2633,6 @@ void AttPosEKF::ConstrainVariances() P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain terrain offset variance - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -2655,7 +2650,6 @@ void AttPosEKF::ConstrainStates() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion for (unsigned i = 0; i <= 3; i++) { @@ -2673,7 +2667,8 @@ void AttPosEKF::ConstrainStates() } // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + // NOT FOR FLIGHT : Upper value of 0.0 is a temporary fix to get around lack of range finder data during development testing + states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec for (unsigned i = 10; i <= 12; i++) { @@ -2699,9 +2694,6 @@ void AttPosEKF::ConstrainStates() states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } - // Constrain terrain offset - states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); - } void AttPosEKF::ForceSymmetry() @@ -3162,12 +3154,14 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[19] = magBias.x; // Magnetic Field Bias X states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - states[22] = 0.0f; // terrain height ResetVelocity(); ResetPosition(); ResetHeight(); + // initialise focal length scale factor estimator states + flowStates[0] = 1.0f; + statesInitialised = true; // initialise the covariance matrix @@ -3223,6 +3217,9 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + // initialise the variables for the focal length scale factor to unity + flowStates[0] = 1.0f; + correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); @@ -3230,12 +3227,9 @@ void AttPosEKF::ZeroVariables() dVelIMU.zero(); lastGyroOffset.zero(); - windSpdFiltNorth = 0.0f; - windSpdFiltEast = 0.0f; - // setting the altitude to zero will give us a higher - // gain to adjust faster in the first step - windSpdFiltAltitude = 0.0f; - windSpdFiltClimb = 0.0f; + // initialise states used by OpticalFlowEKF + flowStates[0] = 1.0f; + flowStates[1] = 0.0f; for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index a607955a8..b1d71bd74 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -2,7 +2,7 @@ #include "estimator_utilities.h" -const unsigned int n_states = 23; +const unsigned int n_states = 22; const unsigned int data_buffer_size = 50; class AttPosEKF { @@ -29,10 +29,6 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - float a1; // optical flow sensor misalgnment angle about X axis (rad) - float a2; // optical flow sensor misalgnment angle about Y axis (rad) - float a3; // optical flow sensor misalgnment angle about Z axis (rad) - float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -59,9 +55,6 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; - a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) - a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) - a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -69,7 +62,6 @@ public: dVelBiasSigma = 1e-4f; magEarthSigma = 3.0e-4f; magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma vneSigma = 0.2f; vdSigma = 0.3f; @@ -82,12 +74,13 @@ public: accelProcessNoise = 0.5f; gndHgtSigma = 0.1f; // terrain gradient 1-sigma - R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 + R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2 flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check - minFlowRng = 0.01f; //minimum range between ground and flow sensor - moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate + rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check + minFlowRng = 0.3f; //minimum range between ground and flow sensor + moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate + } struct mag_state_struct { @@ -134,6 +127,7 @@ public: float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time + float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -145,7 +139,7 @@ public: Vector3f lastGyroOffset; // Last gyro offset Vector3f delAngTotal; - Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow Mat3f Tnb; // transformation amtrix from NED to body coordinates Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) @@ -157,10 +151,6 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates - float windSpdFiltNorth; // average wind speed north component - float windSpdFiltEast; // average wind speed east component - float windSpdFiltAltitude; // the last altitude used to filter wind speed - float windSpdFiltClimb; // filtered climb rate uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -175,7 +165,8 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float losData[2]; // optical flow LOS rate measurements (rad/sec) + float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec) + float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) @@ -190,6 +181,8 @@ public: bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used + uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle // GPS input data variables double gpsLat; @@ -217,6 +210,7 @@ public: bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused + bool useGPS; // boolean true if GPS data is being used bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used @@ -267,11 +261,9 @@ void FuseMagnetometer(); void FuseAirspeed(); -void FuseRangeFinder(); - void FuseOptFlow(); -void GroundEKF(); +void OpticalFlowEKF(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - * + *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms); */ int RecallStates(float *statesForFusion, uint64_t msec); +void RecallOmega(float *omegaForFusion, uint64_t msec); + void ResetStoredStates(); void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); @@ -325,7 +319,7 @@ void CovarianceInit(); void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -float ConstrainFloat(float val, float min, float max); +float ConstrainFloat(float val, float min, float maxf); void ConstrainVariances(); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 843dde978..f51962113 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator_23states.cpp \ + estimator_22states.cpp \ estimator_utilities.cpp EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e0b61e2e2..00c2080a0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -59,7 +59,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> @@ -550,7 +554,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } } @@ -620,7 +624,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 74249c9c5..0bdc285e7 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -816,7 +816,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -963,7 +963,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -974,7 +974,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, @@ -987,7 +987,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); @@ -1140,7 +1140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed); } - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && @@ -1235,12 +1235,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset landing state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LAND) { reset_landing_state(); } /* reset takeoff/launch state */ - if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { + if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1341,7 +1341,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp new file mode 100644 index 000000000..8e5bcf7ba --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include "FixedwingLandDetector.h" + +#include <cmath> +#include <drivers/drv_hrt.h> + +FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), + _paramHandle(), + _params(), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) +{ + _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); +} + +void FixedwingLandDetector::initialize() +{ + // Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool FixedwingLandDetector::update() +{ + // First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + // TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + // crude land detector for fixedwing + if (_velocity_xy_filtered < _params.maxVelocity + && _velocity_z_filtered < _params.maxClimbRate + && _airspeed_filtered < _params.maxAirSpeed) { + + // these conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } + + } else { + // reset land detect trigger + _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; + } + + return landDetected; +} + +void FixedwingLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h new file mode 100644 index 000000000..0e9c092ee --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 FixedwingLandDetector.h + * Land detection algorithm for fixedwing + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> + +class FixedwingLandDetector : public LandDetector +{ +public: + FixedwingLandDetector(); + +protected: + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxVelocity; + param_t maxClimbRate; + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxVelocity; + float maxClimbRate; + float maxAirSpeed; + } _params; + +private: + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + int _parameterSub; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp new file mode 100644 index 000000000..a4fbb9861 --- /dev/null +++ b/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 LandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <morten@lysgaard.no> + */ + +#include "LandDetector.h" +#include <unistd.h> //usleep +#include <drivers/drv_hrt.h> + +LandDetector::LandDetector() : + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) +{ + // ctor +} + +LandDetector::~LandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +void LandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void LandDetector::start() +{ + // make sure this method has not already been called by another thread + if (isRunning()) { + return; + } + + // advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + // initialize land detection algorithm + initialize(); + + // task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; + + while (isRunning()) { + + bool landDetected = update(); + + // publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; + + // publish the land detected broadcast + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + // limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + // check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h new file mode 100644 index 000000000..09db6e474 --- /dev/null +++ b/src/modules/land_detector/LandDetector.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 LandDetector.h + * Abstract interface for land detector algorithms + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#ifndef __LAND_DETECTOR_H__ +#define __LAND_DETECTOR_H__ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_land_detected.h> + +class LandDetector +{ +public: + + LandDetector(); + virtual ~LandDetector(); + + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); + +protected: + + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; + + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; + + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + +protected: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + +private: + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp new file mode 100644 index 000000000..277cb9363 --- /dev/null +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * 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 MulticopterLandDetector.cpp + * Land detection algorithm for multicopters + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <morten@lysgaard.no> + */ + +#include "MulticopterLandDetector.h" + +#include <cmath> +#include <drivers/drv_hrt.h> + +MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), + _paramHandle(), + _params(), + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + _parameterSub(-1), + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + _landTimer(0) +{ + _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); +} + +void MulticopterLandDetector::initialize() +{ + // subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + + // download parameters + updateParameterCache(true); +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +bool MulticopterLandDetector::update() +{ + // first poll for new data from our subscriptions + updateSubscriptions(); + + // only trigger flight conditions if we are armed + if (!_arming.armed) { + return true; + } + + const uint64_t now = hrt_absolute_time(); + + // check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; + + // check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; + + // next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; + + // check if thrust output is minimal (about half of default) + bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + // sensed movement, so reset the land detector + _landTimer = now; + return false; + } + + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; +} + +void MulticopterLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + } +} diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h new file mode 100644 index 000000000..7bb7f1a90 --- /dev/null +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 MulticopterLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen <jnsn.johan@gmail.com> + * @author Morten Lysgaard <morten@lysgaard.no> + */ + +#ifndef __MULTICOPTER_LAND_DETECTOR_H__ +#define __MULTICOPTER_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> + +class MulticopterLandDetector : public LandDetector +{ +public: + MulticopterLandDetector(); + +protected: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxClimbRate; + param_t maxVelocity; + param_t maxRotation; + param_t maxThrottle; + } _paramHandle; + + struct { + float maxClimbRate; + float maxVelocity; + float maxRotation; + float maxThrottle; + } _params; + +private: + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; + int _parameterSub; + + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ +}; + +#endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp new file mode 100644 index 000000000..1e43e7ad5 --- /dev/null +++ b/src/modules/land_detector/land_detector_main.cpp @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * 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 land_detector_main.cpp + * Land detection algorithm + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include <unistd.h> //usleep +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <drivers/drv_hrt.h> +#include <systemlib/systemlib.h> //Scheduler +#include <systemlib/err.h> //print to console + +#include "FixedwingLandDetector.h" +#include "MulticopterLandDetector.h" + +//Function prototypes +static int land_detector_start(const char *mode); +static void land_detector_stop(); + +/** + * land detector app start / stop handling function + * This makes the land detector module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); + +//Private variables +static LandDetector *land_detector_task = nullptr; +static int _landDetectorTaskID = -1; +static char _currentMode[12]; + +/** +* Deamon thread function +**/ +static void land_detector_deamon_thread(int argc, char *argv[]) +{ + land_detector_task->start(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void land_detector_stop() +{ + if (land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (land_detector_task->isRunning()); + + + delete land_detector_task; + land_detector_task = nullptr; + _landDetectorTaskID = -1; + errx(0, "land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int land_detector_start(const char *mode) +{ + if (land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + if (!strcmp(mode, "fixedwing")) { + land_detector_task = new FixedwingLandDetector(); + + } else if (!strcmp(mode, "multicopter")) { + land_detector_task = new MulticopterLandDetector(); + + } else { + errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + return -1; + } + + //Check if alloc worked + if (land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1200, + (main_t)&land_detector_deamon_thread, + nullptr); + + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + + while (!land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if (hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + land_detector_stop(); + exit(1); + } + } + + printf("\n"); + + //Remember current active mode + strncpy(_currentMode, mode, 12); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); + exit(0); + } + + if (argc >= 2 && !strcmp(argv[1], "start")) { + land_detector_start(argv[2]); + } + + if (!strcmp(argv[1], "stop")) { + land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (land_detector_task) { + + if (land_detector_task->isRunning()) { + warnx("running (%s)", _currentMode); + + } else { + errx(1, "exists, but not running (%s)", _currentMode); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); + return 1; +} diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c new file mode 100644 index 000000000..0302bc7c1 --- /dev/null +++ b/src/modules/land_detector/land_detector_params.c @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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 land_detector.c + * Land detector algorithm parameters. + * + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include <systemlib/param/param.h> + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); + +/** + * Multicopter max rotation + * + * Maximum allowed around each axis to trigger a land (radians per second) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f); + +/** + * Multicopter max throttle + * + * Maximum actuator output on throttle before triggering a land + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); + +/** + * Airspeed max + * + * Maximum airspeed allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk new file mode 100644 index 000000000..e08a4b7a8 --- /dev/null +++ b/src/modules/land_detector/module.mk @@ -0,0 +1,13 @@ +# +# Land detector +# + +MODULE_COMMAND = land_detector + +SRCS = land_detector_main.cpp \ + land_detector_params.c \ + LandDetector.cpp \ + MulticopterLandDetector.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ -Os diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df..6bd0c7bce 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->get_topic() == topic) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { /* already subscribed */ return sub; } } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); LL_APPEND(_subscriptions, sub_new); @@ -1431,7 +1431,7 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status.hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ad5e5001b..baaa7bc13 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -171,7 +171,7 @@ public: void handle_message(const mavlink_message_t *msg); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); int get_instance_id(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c7..be7d91c65 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -103,13 +103,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = 0; /* HIL */ - if (status->hil_state == HIL_STATE_ON) { + if (status->hil_state == vehicle_status_s::HIL_STATE_ON) { *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED + || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -121,31 +121,31 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; break; - case NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; break; - case NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -153,7 +153,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -161,9 +161,9 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -171,11 +171,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: /* fallthrough */ - case NAVIGATION_STATE_DESCEND: + case vehicle_status_s::NAVIGATION_STATE_DESCEND: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -183,7 +183,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; @@ -191,19 +191,19 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; break; - case NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; break; - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; break; - case NAVIGATION_STATE_MAX: + case vehicle_status_s::NAVIGATION_STATE_MAX: /* this is an unused case, ignore */ break; @@ -212,21 +212,21 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_custom_mode = custom_mode.data; /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + if (status->arming_state == vehicle_status_s::ARMING_STATE_INIT + || status->arming_state == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; } else { @@ -1342,14 +1342,7 @@ protected: _act_sub(nullptr), _act_time(0) { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N); } void send(const hrt_abstime t) @@ -1424,7 +1417,7 @@ protected: _status_time(0), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _pos_sp_triplet_time(0), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_time(0) {} @@ -1438,7 +1431,7 @@ protected: updated |= _pos_sp_triplet_sub->update(&_pos_sp_triplet_time, &pos_sp_triplet); updated |= _status_sub->update(&_status_time, &status); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -2205,7 +2198,7 @@ protected: msg.param2 = 0; msg.param3 = 0; /* set camera capture ON/OFF depending on arming state */ - msg.param4 = (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) ? 1 : 0; + msg.param4 = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) ? 1 : 0; msg.param5 = 0; msg.param6 = 0; msg.param7 = 0; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 734f0903a..315776e29 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,10 +46,11 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) : next(nullptr), _topic(topic), - _fd(orb_subscribe(_topic)), + _instance(instance), + _fd(orb_subscribe_multi(_topic, instance)), _published(false) { } @@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const return _topic; } +int +MavlinkOrbSubscription::get_instance() const +{ + return _instance; +} + bool MavlinkOrbSubscription::update(uint64_t *time, void* data) { diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 7af454df6..5394e5097 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,7 +50,7 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; ///< pointer to next subscription in list - MavlinkOrbSubscription(const orb_id_t topic); + MavlinkOrbSubscription(const orb_id_t topic, int instance); ~MavlinkOrbSubscription(); /** @@ -77,9 +77,11 @@ public: */ bool is_published(); orb_id_t get_topic() const; + int get_instance() const; private: const orb_id_t _topic; ///< topic metadata + const int _instance; ///< get topic instance int _fd; ///< subscription handle bool _published; ///< topic was ever published diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ce3f2ae0e..4d7b35f03 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), status{}, hil_local_pos{}, + hil_land_detector{}, _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), @@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _land_detector_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -433,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; @@ -615,7 +617,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ @@ -984,10 +986,10 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; - if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = offset_ns; warnx("[timesync] Hard setting offset."); } else { @@ -1041,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); } } @@ -1063,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } @@ -1084,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); } else { - orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); } } @@ -1102,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); } else { - orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); } } @@ -1353,9 +1356,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; - if (_local_pos_pub < 0) { _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); @@ -1364,6 +1364,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + /* accelerometer */ { struct accel_report accel; @@ -1379,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4afc9b372..699996860 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -50,6 +50,7 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_land_detected.h> #include <uORB/topics/home_position.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/offboard_control_setpoint.h> @@ -145,6 +146,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_land_detected_s hil_land_detector; struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; @@ -171,6 +173,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + orb_advert_t _land_detector_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; 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 228f142d9..f20bba52e 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 <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Anton Babushkin <anton.babushkin@me.com> @@ -63,7 +67,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_status.h> diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 822a504b5..1e40c2b05 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -104,15 +104,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* * do subscriptions */ - _v_att = PX4_SUBSCRIBE(_n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); - _v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0); - _v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0); - _v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0); - _parameter_update = PX4_SUBSCRIBE(_n, parameter_update, - MulticopterAttitudeControl::handle_parameter_update, this, 1000); - _manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0); - _armed = PX4_SUBSCRIBE(_n, actuator_armed, 0); - _v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0); + _v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0); + _v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0); + _v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0); + _parameter_update = _n.subscribe<px4_parameter_update>( + &MulticopterAttitudeControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); + _armed = _n.subscribe<px4_actuator_armed>(0); + _v_status = _n.subscribe<px4_vehicle_status>(0); } @@ -180,12 +180,12 @@ MulticopterAttitudeControl::parameters_update() return OK; } -void MulticopterAttitudeControl::handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg) +void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg) { parameters_update(); } -void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) { +void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { perf_begin(_loop_perf); @@ -202,95 +202,98 @@ void MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi dt = 0.02f; } - if (_v_control_mode->get().flag_control_attitude_enabled) { + if (_v_control_mode->data().flag_control_attitude_enabled) { control_attitude(dt); /* publish the attitude setpoint if needed */ - if (_publish_att_sp && _v_status->get().is_rotary_wing) { - _v_att_sp_mod.timestamp = px4::get_time_micros(); + if (_publish_att_sp && _v_status->data().is_rotary_wing) { + _v_att_sp_mod.data().timestamp = px4::get_time_micros(); if (_att_sp_pub != nullptr) { _att_sp_pub->publish(_v_att_sp_mod); } else { - _att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint); + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); } } /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x, - _manual_control_sp->get().r).emult(_params.acro_rate_max); - _thrust_sp = _manual_control_sp->get().z; + _rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x, + _manual_control_sp->data().r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp->data().z; /* reset yaw setpoint after ACRO */ _reset_yaw_sp = true; /* publish attitude rates setpoint */ - _v_rates_sp_mod.roll = _rates_sp(0); - _v_rates_sp_mod.pitch = _rates_sp(1); - _v_rates_sp_mod.yaw = _rates_sp(2); - _v_rates_sp_mod.thrust = _thrust_sp; - _v_rates_sp_mod.timestamp = px4::get_time_micros(); + _v_rates_sp_mod.data().roll = _rates_sp(0); + _v_rates_sp_mod.data().pitch = _rates_sp(1); + _v_rates_sp_mod.data().yaw = _rates_sp(2); + _v_rates_sp_mod.data().thrust = _thrust_sp; + _v_rates_sp_mod.data().timestamp = px4::get_time_micros(); if (_v_rates_sp_pub != nullptr) { _v_rates_sp_pub->publish(_v_rates_sp_mod); } else { - if (_v_status->get().is_vtol) { - _v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>(); } else { - _v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint); + _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>(); } } } else { /* attitude controller disabled, poll rates setpoint topic */ - _rates_sp(0) = _v_rates_sp->get().roll; - _rates_sp(1) = _v_rates_sp->get().pitch; - _rates_sp(2) = _v_rates_sp->get().yaw; - _thrust_sp = _v_rates_sp->get().thrust; + _rates_sp(0) = _v_rates_sp->data().roll; + _rates_sp(1) = _v_rates_sp->data().pitch; + _rates_sp(2) = _v_rates_sp->data().yaw; + _thrust_sp = _v_rates_sp->data().thrust; } } - if (_v_control_mode->get().flag_control_rates_enabled) { + if (_v_control_mode->data().flag_control_rates_enabled) { control_attitude_rates(dt); /* publish actuator controls */ - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.timestamp = px4::get_time_micros(); + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().timestamp = px4::get_time_micros(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { _actuators_0_pub->publish(_actuators); } else { - if (_v_status->get().is_vtol) { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc); + if (_v_status->data().is_vtol) { + //XXX add a second publisher? + // _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>(); } else { - _actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0); + _actuators_0_pub = _n.advertise<px4_actuator_controls_0>(); } } } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index bff5289fd..95d608684 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -59,7 +59,6 @@ #include <math.h> #include <poll.h> #include <systemlib/perf_counter.h> -// #include <systemlib/systemlib.h> #include <systemlib/circuit_breaker.h> #include <lib/mathlib/mathlib.h> @@ -80,8 +79,8 @@ public: ~MulticopterAttitudeControl(); /* Callbacks for topics */ - void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); - void handle_parameter_update(const PX4_TOPIC_T(parameter_update) &msg); + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); void spin() { _n.spin(); } @@ -89,9 +88,9 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ - px4::Publisher * _att_sp_pub; /**< attitude setpoint publication */ - px4::Publisher * _v_rates_sp_pub; /**< rate setpoint publication */ - px4::Publisher * _actuators_0_pub; /**< attitude actuator controls publication */ + px4::Publisher<px4_vehicle_attitude_setpoint> * _att_sp_pub; /**< attitude setpoint publication */ + px4::Publisher<px4_vehicle_rates_setpoint> * _v_rates_sp_pub; /**< rate setpoint publication */ + px4::Publisher<px4_actuator_controls_0> * _actuators_0_pub; /**< attitude actuator controls publication */ px4::NodeHandle _n; diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index aff59778e..1f0d88bcd 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -55,13 +55,12 @@ using namespace std; #endif MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + _v_att_sp_mod(), + _v_rates_sp_mod(), + _actuators(), _publish_att_sp(false) { - memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod)); - memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod)); - memset(&_actuators, 0, sizeof(_actuators)); - _params.att_p.zero(); _params.rate_p.zero(); _params.rate_i.zero(); @@ -92,28 +91,29 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _publish_att_sp = false; - if (_v_control_mode->get().flag_control_manual_enabled) { + if (_v_control_mode->data().flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ - if (_v_control_mode->get().flag_control_velocity_enabled - || _v_control_mode->get().flag_control_climb_rate_enabled) { + if (_v_control_mode->data().flag_control_velocity_enabled + || _v_control_mode->data().flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); } - if (!_v_control_mode->get().flag_control_climb_rate_enabled) { + if (!_v_control_mode->data().flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp_mod.thrust = _manual_control_sp->get().z; + _v_att_sp_mod.data().thrust = _manual_control_sp->data().z; _publish_att_sp = true; } - if (!_armed->get().armed) { + if (!_armed->data().armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } /* move yaw setpoint in all modes */ - if (_v_att_sp_mod.thrust < 0.1f) { + if (_v_att_sp_mod.data().thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ @@ -121,71 +121,72 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max; - _v_att_sp_mod.yaw_body = _wrap_pi( - _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt); + yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max; + _v_att_sp_mod.data().yaw_body = _wrap_pi( + _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); - float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw); + float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw); if (yaw_offs < -yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { - _v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max); + _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max); } - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } - /* reset yaw setpint to current position if needed */ + /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; - _v_att_sp_mod.yaw_body = _v_att->get().yaw; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().yaw_body = _v_att->data().yaw; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } - if (!_v_control_mode->get().flag_control_velocity_enabled) { + if (!_v_control_mode->data().flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max; - _v_att_sp_mod.pitch_body = -_manual_control_sp->get().x + _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max; + _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max; - _v_att_sp_mod.R_valid = false; + _v_att_sp_mod.data().R_valid = false; // _publish_att_sp = true; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ - memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod)); + //XXX get rid of memcpy + memcpy(&(_v_att_sp_mod.data()), &_v_att_sp->data(), sizeof(_v_att_sp_mod.data())); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } - _thrust_sp = _v_att_sp_mod.thrust; + _thrust_sp = _v_att_sp_mod.data().thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; - if (_v_att_sp_mod.R_valid) { + if (_v_att_sp_mod.data().R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp_mod.R_body[0]); + R_sp.set(&_v_att_sp_mod.data().R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body, - _v_att_sp_mod.yaw_body); + R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body, + _v_att_sp_mod.data().yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0], - sizeof(_v_att_sp_mod.R_body)); - _v_att_sp_mod.R_valid = true; + memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0], + sizeof(_v_att_sp_mod.data().R_body)); + _v_att_sp_mod.data().R_valid = true; } /* rotation matrix for current state */ math::Matrix<3, 3> R; - R.set(_v_att->get().R); + R.set(_v_att->data().R); /* all input data is ready, run controller itself */ @@ -266,15 +267,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed->get().armed || !_v_status->get().is_rotary_wing) { + if (!_armed->data().armed || !_v_status->data().is_rotary_wing) { _rates_int.zero(); } /* current body angular rates */ math::Vector < 3 > rates; - rates(0) = _v_att->get().rollspeed; - rates(1) = _v_att->get().pitchspeed; - rates(2) = _v_att->get().yawspeed; + rates(0) = _v_att->data().rollspeed; + rates(1) = _v_att->data().pitchspeed; + rates(2) = _v_att->data().yawspeed; /* angular rates error */ math::Vector < 3 > rates_err = _rates_sp - rates; @@ -302,8 +303,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) void MulticopterAttitudeControlBase::set_actuator_controls() { - _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; } diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h index cf4c726a7..124147079 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h @@ -62,6 +62,8 @@ #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f +using namespace px4; + class MulticopterAttitudeControlBase { public: @@ -86,20 +88,20 @@ public: void set_actuator_controls(); protected: - px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att; /**< vehicle attitude */ - px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp; /**< vehicle attitude setpoint */ - px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp; /**< vehicle rates setpoint */ - px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode; /**< vehicle control mode */ - px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update; /**< parameter update */ - px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp; /**< manual control setpoint */ - px4::PX4_SUBSCRIBER(actuator_armed) *_armed; /**< actuator arming status */ - px4::PX4_SUBSCRIBER(vehicle_status) *_v_status; /**< vehicle status */ - - PX4_TOPIC_T(vehicle_attitude_setpoint) _v_att_sp_mod; /**< modified vehicle attitude setpoint + px4::Subscriber<px4_vehicle_attitude> *_v_att; /**< vehicle attitude */ + px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */ + px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp; /**< vehicle rates setpoint */ + px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode; /**< vehicle control mode */ + px4::Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ + px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ + px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ + px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */ + + px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint that gets published eventually */ - PX4_TOPIC_T(vehicle_rates_setpoint) _v_rates_sp_mod; /**< vehicle rates setpoint + px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint that gets published eventually*/ - PX4_TOPIC_T(actuator_controls) _actuators; /**< actuator controls */ + px4_actuator_controls_0 _actuators; /**< actuator controls */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp index 67ebe64cd..5a79a8c6b 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp @@ -52,77 +52,11 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include <string.h> -#include <cstdlib> #include "mc_att_control.h" -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ -namespace px4 -{ -bool task_should_exit = false; -} - -using namespace px4; - -PX4_MAIN_FUNCTION(mc_att_control_m); - -#if !defined(__PX4_ROS) -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - -extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); -int mc_att_control_m_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "usage: mc_att_control_m {start|stop|status}"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - task_should_exit = false; - - daemon_task = task_spawn_cmd("mc_att_control_m", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 3000, - mc_att_control_m_task_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - task_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - warnx("unrecognized command"); - return 1; -} -#endif +bool thread_running = false; /**< Deamon status flag */ -PX4_MAIN_FUNCTION(mc_att_control_m) +int main(int argc, char **argv) { px4::init(argc, argv, "mc_att_control_m"); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp new file mode 100644 index 000000000..c2b847075 --- /dev/null +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_m_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]); +int mc_att_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_att_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_att_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk index c61ba18b4..959f6492b 100644 --- a/src/modules/mc_att_control_multiplatform/module.mk +++ b/src/modules/mc_att_control_multiplatform/module.mk @@ -38,6 +38,7 @@ MODULE_COMMAND = mc_att_control_m SRCS = mc_att_control_main.cpp \ + mc_att_control_start_nuttx.cpp \ mc_att_control.cpp \ mc_att_control_base.cpp \ mc_att_control_params.c 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 bf65d2805..b9692ffbf 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) { @@ -755,7 +768,7 @@ MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ math::Vector<3> prev_sp; map_projection_project(&_ref_pos, @@ -985,7 +998,7 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1024,7 +1037,7 @@ MulticopterPositionControl::task_main() } /* use constant descend rate when landing, ignore altitude setpoint */ - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } @@ -1111,7 +1124,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp new file mode 100644 index 000000000..a46163e6f --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -0,0 +1,949 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 mc_pos_control.cpp + * Multicopter position controller. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "mc_pos_control.h" +#include "mc_pos_control_params.h" + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f +#define MIN_DIST 0.01f + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + _mavlink_fd(-1), + +/* publications */ + _att_sp_pub(nullptr), + _local_pos_sp_pub(nullptr), + _global_vel_sp_pub(nullptr), + + _ref_alt(0.0f), + _ref_timestamp(0), + + _reset_pos_sp(true), + _reset_alt_sp(true), + _mode_auto(false), + _thrust_int(), + _R() +{ + memset(&_ref_pos, 0, sizeof(_ref_pos)); + + /* + * Do subscriptions + */ + _att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0); + _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0); + _control_mode = _n.subscribe<px4_vehicle_control_mode>(0); + _parameter_update = _n.subscribe<px4_parameter_update>( + &MulticopterPositionControl::handle_parameter_update, this, 1000); + _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); + _armed = _n.subscribe<px4_actuator_armed>(0); + _local_pos = _n.subscribe<px4_vehicle_local_position>(0); + _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0); + _local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0); + _global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0); + + + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _pos_sp.zero(); + _vel.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + _vel_ff.zero(); + _sp_move_rate.zero(); + + _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); + _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); + _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); + _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); + _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); + _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); + _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); + _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); + _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); + _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); + _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); + _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); + _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); + _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); + _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); + _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); + _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); + + /* fetch initial parameter values */ + parameters_update(); + + _R.identity(); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ +} + +int +MulticopterPositionControl::parameters_update() +{ + PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); + PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); + PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); + PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); + PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); + + float v; + PX4_PARAM_GET(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + PX4_PARAM_GET(_params_handles.z_p, &v); + _params.pos_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + PX4_PARAM_GET(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + PX4_PARAM_GET(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + PX4_PARAM_GET(_params_handles.xy_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + PX4_PARAM_GET(_params_handles.z_ff, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + + return OK; +} + + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::update_ref() +{ + if (_local_pos->data().ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp = 0.0f; + + if (_ref_timestamp != 0) { + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); + } + + /* update local projection reference */ + map_projection_init(&_ref_pos, _local_pos->data().ref_lat, _local_pos->data().ref_lon); + _ref_alt = _local_pos->data().ref_alt; + + if (_ref_timestamp != 0) { + /* reproject position setpoint to new reference */ + map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos->data().ref_timestamp; + } +} + +void +MulticopterPositionControl::reset_pos_sp() +{ + if (_reset_pos_sp) { + _reset_pos_sp = false; + /* shift position setpoint to make attitude setpoint continuous */ + _pos_sp(0) = _pos(0); //+ (_vel(0) - PX4_R(_att_sp_msg.data().R_body, 0, 2) * _att_sp_msg.data().thrust / _params.vel_p(0) + // - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + _pos_sp(1) = _pos(1); //+ (_vel(1) - PX4_R(_att_sp_msg.data().R_body, 1, 2) * _att_sp_msg.data().thrust / _params.vel_p(1) + // - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); + PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1)); + } +} + +void +MulticopterPositionControl::reset_alt_sp() +{ + if (_reset_alt_sp) { + _reset_alt_sp = false; + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + + //XXX: port this once a mavlink like interface is available + // mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); + PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2)); + } +} + +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_manual(float dt) +{ + _sp_move_rate.zero(); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* move altitude setpoint with throttle stick */ + _sp_move_rate(2) = -scale_control(_manual_control_sp->data().z - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* move position setpoint with roll/pitch stick */ + _sp_move_rate(0) = _manual_control_sp->data().x; + _sp_move_rate(1) = _manual_control_sp->data().y; + } + + /* limit setpoint move rate */ + float sp_move_norm = _sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + _sp_move_rate /= sp_move_norm; + } + + /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); + + if (_control_mode->data().flag_control_altitude_enabled) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + } + + if (_control_mode->data().flag_control_position_enabled) { + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode->data().flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode->data().flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + +void +MulticopterPositionControl::control_offboard(float dt) +{ + if (_pos_sp_triplet->data().current.valid) { + if (_control_mode->data().flag_control_position_enabled && _pos_sp_triplet->data().current.position_valid) { + /* control position */ + _pos_sp(0) = _pos_sp_triplet->data().current.x; + _pos_sp(1) = _pos_sp_triplet->data().current.y; + } else if (_control_mode->data().flag_control_velocity_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* control velocity */ + /* reset position setpoint to current position if needed */ + reset_pos_sp(); + + /* set position setpoint move rate */ + _sp_move_rate(0) = _pos_sp_triplet->data().current.vx; + _sp_move_rate(1) = _pos_sp_triplet->data().current.vy; + } + + if (_pos_sp_triplet->data().current.yaw_valid) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } else if (_pos_sp_triplet->data().current.yawspeed_valid) { + _att_sp_msg.data().yaw_body = _att_sp_msg.data().yaw_body + _pos_sp_triplet->data().current.yawspeed * dt; + } + + if (_control_mode->data().flag_control_altitude_enabled && _pos_sp_triplet->data().current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet->data().current.z; + } else if (_control_mode->data().flag_control_climb_rate_enabled && _pos_sp_triplet->data().current.velocity_valid) { + /* reset alt setpoint to current altitude if needed */ + reset_alt_sp(); + + /* set altitude setpoint move rate */ + _sp_move_rate(2) = _pos_sp_triplet->data().current.vz; + } + + /* feed forward setpoint move rate with weight vel_ff */ + _vel_ff = _sp_move_rate.emult(_params.vel_ff); + + /* move position setpoint */ + _pos_sp += _sp_move_rate * dt; + + } else { + reset_pos_sp(); + reset_alt_sp(); + } +} + +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto(float dt) +{ + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + + if (_pos_sp_triplet->data().current.valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + + /* project setpoint to local frame */ + math::Vector<3> curr_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().current.lat, _pos_sp_triplet->data().current.lon, + &curr_sp.data[0], &curr_sp.data[1]); + curr_sp(2) = -(_pos_sp_triplet->data().current.alt - _ref_alt); + + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + + /* by default use current setpoint as is */ + math::Vector<3> pos_sp_s = curr_sp_s; + + if (_pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_POSITION && _pos_sp_triplet->data().previous.valid) { + /* follow "previous - current" line */ + math::Vector<3> prev_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().previous.lat, _pos_sp_triplet->data().previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet->data().previous.alt - _ref_alt); + + if ((curr_sp - prev_sp).length() > MIN_DIST) { + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet->data().next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet->data().next.lat, _pos_sp_triplet->data().next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet->data().next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); + + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; + + /* cos(b), b = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a_curr_next /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + pos_sp_s += pos_ff; + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; + } + + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); + } + } + } + } + + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet->data().current.yaw)) { + _att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw; + } + + } else { + /* no waypoint, do nothing, setpoint was already reset */ + } +} + +void MulticopterPositionControl::handle_parameter_update(const px4_parameter_update &msg) +{ + parameters_update(); +} + +void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) +{ + static bool reset_int_z = true; + static bool reset_int_z_manual = false; + static bool reset_int_xy = true; + static bool was_armed = false; + static uint64_t t_prev = 0; + + uint64_t t = get_time_micros(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f; + t_prev = t; + + if (_control_mode->data().flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + _reset_pos_sp = true; + _reset_alt_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode->data().flag_armed; + + update_ref(); + + if (_control_mode->data().flag_control_altitude_enabled || + _control_mode->data().flag_control_position_enabled || + _control_mode->data().flag_control_climb_rate_enabled || + _control_mode->data().flag_control_velocity_enabled) { + + _pos(0) = _local_pos->data().x; + _pos(1) = _local_pos->data().y; + _pos(2) = _local_pos->data().z; + + _vel(0) = _local_pos->data().vx; + _vel(1) = _local_pos->data().vy; + _vel(2) = _local_pos->data().vz; + + _vel_ff.zero(); + _sp_move_rate.zero(); + + /* select control source */ + if (_control_mode->data().flag_control_manual_enabled) { + /* manual control */ + control_manual(dt); + _mode_auto = false; + + } else if (_control_mode->data().flag_control_offboard_enabled) { + /* offboard control */ + control_offboard(dt); + _mode_auto = false; + + } else { + /* AUTO */ + control_auto(dt); + } + + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise<px4_vehicle_local_position_setpoint>(); + } + + + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { + /* idle state, don't run controller and set zero thrust */ + _R.identity(); + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + _att_sp_msg.data().yaw_body = _att->data().yaw; + _att_sp_msg.data().thrust = 0.0f; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + + } else { + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + + _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + + if (!_control_mode->data().flag_control_altitude_enabled) { + _reset_alt_sp = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode->data().flag_control_position_enabled) { + _reset_pos_sp = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + /* use constant descend rate when landing, ignore altitude setpoint */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + _global_vel_sp_msg.data().vx = _vel_sp(0); + _global_vel_sp_msg.data().vy = _vel_sp(1); + _global_vel_sp_msg.data().vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub != nullptr) { + _global_vel_sp_pub->publish(_global_vel_sp_msg); + + } else { + _global_vel_sp_pub = _n.advertise<px4_vehicle_global_velocity_setpoint>(); + } + + if (_control_mode->data().flag_control_climb_rate_enabled || _control_mode->data().flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode->data().flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual_control_sp->data().z; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + _thrust_int(2) = -i; + } + + } else { + reset_int_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + _thrust_int(0) = 0.0f; + _thrust_int(1) = 0.0f; + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + _thrust_int; + + if (!_control_mode->data().flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode->data().flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode->data().flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + float tilt_max = _params.tilt_max_air; + + /* adjust limits for landing mode */ + if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && + _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.tilt_max_land; + + if (thr_min < 0.0f) { + thr_min = 0.0f; + } + } + + /* limit min lift */ + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + if (_control_mode->data().flag_control_velocity_enabled) { + /* limit max tilt */ + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } + } + + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) { + att_comp = 1.0f / PX4_R(_att->data().R, 2, 2); + + } else if (PX4_R(_att->data().R, 2, 2) > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f; + saturation_z = true; + + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode->data().flag_control_velocity_enabled && !saturation_xy) { + _thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + _thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode->data().flag_control_climb_rate_enabled && !saturation_z) { + _thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (_thrust_int(2) > 0.0f) { + _thrust_int(2) = 0.0f; + } + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode->data().flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp_msg.data().yaw_body), cosf(_att_sp_msg.data().yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + _R(i, 0) = body_x(i); + _R(i, 1) = body_y(i); + _R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = _R.to_euler(); + _att_sp_msg.data().roll_body = euler(0); + _att_sp_msg.data().pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode->data().flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + _R.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp_msg.data().R_body[0], _R.data, sizeof(_att_sp_msg.data().R_body)); + _att_sp_msg.data().R_valid = true; + + _att_sp_msg.data().roll_body = 0.0f; + _att_sp_msg.data().pitch_body = 0.0f; + } + + _att_sp_msg.data().thrust = thrust_abs; + + _att_sp_msg.data().timestamp = get_time_micros(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + _att_sp_pub->publish(_att_sp_msg); + + } else { + _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>(); + } + + } else { + reset_int_z = true; + } + } + + } else { + /* position controller disabled, reset setpoints */ + _reset_alt_sp = true; + _reset_pos_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h new file mode 100644 index 000000000..05bd1387b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * 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 mc_pos_control.h + * Multicopter position controller. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#pragma once + +#include <px4.h> +#include <cstdio> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +// #include <poll.h> +// #include <drivers/drv_hrt.h> +// #include <arch/board/board.h> +// #include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> +#include <lib/geo/geo.h> +// #include <mavlink/mavlink_log.h> + +using namespace px4; + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /* Callbacks for topics */ + void handle_vehicle_attitude(const px4_vehicle_attitude &msg); + void handle_parameter_update(const px4_parameter_update &msg); + + void spin() { _n.spin(); } + +protected: + const float alt_ctl_dz = 0.2f; + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + int _mavlink_fd; /**< mavlink fd */ + + Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */ + Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */ + Publisher<px4_vehicle_global_velocity_setpoint> *_global_vel_sp_pub; /**< vehicle global velocity setpoint publication */ + + + Subscriber<px4_vehicle_attitude> *_att; /**< vehicle attitude */ + Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */ + Subscriber<px4_vehicle_control_mode> *_control_mode; /**< vehicle control mode */ + Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */ + Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */ + Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */ + Subscriber<px4_vehicle_local_position> *_local_pos; /**< local position */ + Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet; /**< local position */ + Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp; /**< local position */ + Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp; /**< local position */ + + px4_vehicle_attitude_setpoint _att_sp_msg; + px4_vehicle_local_position_setpoint _local_pos_sp_msg; + px4_vehicle_global_velocity_setpoint _global_vel_sp_msg; + + px4::NodeHandle _n; + + + struct { + px4_param_t thr_min; + px4_param_t thr_max; + px4_param_t z_p; + px4_param_t z_vel_p; + px4_param_t z_vel_i; + px4_param_t z_vel_d; + px4_param_t z_vel_max; + px4_param_t z_ff; + px4_param_t xy_p; + px4_param_t xy_vel_p; + px4_param_t xy_vel_i; + px4_param_t xy_vel_d; + px4_param_t xy_vel_max; + px4_param_t xy_ff; + px4_param_t tilt_max_air; + px4_param_t land_speed; + px4_param_t tilt_max_land; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max_air; + float land_speed; + float tilt_max_land; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + struct map_projection_reference_s _ref_pos; + float _ref_alt; + uint64_t _ref_timestamp; + + bool _reset_pos_sp; + bool _reset_alt_sp; + bool _mode_auto; + + math::Vector<3> _pos; + math::Vector<3> _pos_sp; + math::Vector<3> _vel; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + math::Vector<3> _vel_ff; + math::Vector<3> _sp_move_rate; + + math::Vector<3> _thrust_int; + math::Matrix<3, 3> _R; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Update reference for local position projection + */ + void update_ref(); + /** + * Reset position setpoint to current position + */ + void reset_pos_sp(); + + /** + * Reset altitude setpoint to current altitude + */ + void reset_alt_sp(); + + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + + /** + * Set position setpoint using manual control + */ + void control_manual(float dt); + + /** + * Set position setpoint using offboard control + */ + void control_offboard(float dt); + + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(float dt); + + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); +}; diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp index 6766bb58a..0b5775736 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_main.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -35,33 +32,32 @@ ****************************************************************************/ /** - * @file vehicle_local_position_setpoint.h - * Definition of the local NED position setpoint uORB topic. + * @file mc_pos_control_main.cpp + * Multicopter position controller. + * + * 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). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_ - -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ +#include "mc_pos_control.h" -struct vehicle_local_position_setpoint_s { - uint64_t timestamp; /**< timestamp of the setpoint */ - float x; /**< in meters NED */ - float y; /**< in meters NED */ - float z; /**< in meters NED */ - float yaw; /**< in radians NED -PI..+PI */ -}; /**< Local position in NED frame */ +bool thread_running = false; /**< Deamon status flag */ -/** - * @} - */ +int main(int argc, char **argv) +{ + px4::init(argc, argv, "mc_pos_control_m"); -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position_setpoint); + PX4_INFO("starting"); + MulticopterPositionControl posctl; + thread_running = true; + posctl.spin(); -#endif + PX4_INFO("exiting."); + thread_running = false; + return 0; +} diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c new file mode 100644 index 000000000..c741a7f0a --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.c @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> + * + * 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) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <px4_defines.h> +#include "mc_pos_control_params.h" +#include <systemlib/param/param.h> + +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MIN); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_THR_MAX); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_P); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_P); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_I); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_D); + +/** + * Maximum vertical velocity + * + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX); + +/** + * Vertical velocity feed forward + * + * Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_Z_FF); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_P); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_P); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_I); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_D); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX); + +/** + * Horizontal velocity feed forward + * + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_XY_FF); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PX4_PARAM_DEFINE_FLOAT(MPC_LAND_SPEED); + diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h index 3bd70272f..fec3bcb7c 100644 --- a/src/platforms/nuttx/px4_publisher.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_params.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,10 +33,29 @@ ****************************************************************************/ /** - * @file px4_publisher.cpp + * @file mc_pos_control_params.h + * Multicopter position controller parameters. * - * PX4 Middleware Wrapper for Publisher + * @author Anton Babushkin <anton.babushkin@me.com> */ -#include <platforms/px4_publisher.h> +#pragma once + +#define PARAM_MPC_THR_MIN_DEFAULT 0.1f +#define PARAM_MPC_THR_MAX_DEFAULT 1.0f +#define PARAM_MPC_Z_P_DEFAULT 1.0f +#define PARAM_MPC_Z_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_Z_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_Z_VEL_D_DEFAULT 0.0f +#define PARAM_MPC_Z_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_Z_FF_DEFAULT 0.5f +#define PARAM_MPC_XY_P_DEFAULT 1.0f +#define PARAM_MPC_XY_VEL_P_DEFAULT 0.1f +#define PARAM_MPC_XY_VEL_I_DEFAULT 0.02f +#define PARAM_MPC_XY_VEL_D_DEFAULT 0.01f +#define PARAM_MPC_XY_VEL_MAX_DEFAULT 5.0f +#define PARAM_MPC_XY_FF_DEFAULT 0.5f +#define PARAM_MPC_TILTMAX_AIR_DEFAULT 45.0f +#define PARAM_MPC_TILTMAX_LND_DEFAULT 15.0f +#define PARAM_MPC_LAND_SPEED_DEFAULT 1.0f diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp new file mode 100644 index 000000000..87996d93b --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control_start_nuttx.cpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 mc_pos_control_m_start_nuttx.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <cstdlib> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +extern bool thread_running; +int daemon_task; /**< Handle of deamon task / thread */ +namespace px4 +{ +bool task_should_exit = false; +} +using namespace px4; + +extern int main(int argc, char **argv); + +extern "C" __EXPORT int mc_pos_control_m_main(int argc, char *argv[]); +int mc_pos_control_m_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: mc_pos_control_m {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + task_should_exit = false; + + daemon_task = task_spawn_cmd("mc_pos_control_m", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 3000, + main, + (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + task_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk new file mode 100644 index 000000000..2852ebbec --- /dev/null +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2013 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. +# +############################################################################ + +# +# Build multicopter position controller +# + +MODULE_COMMAND = mc_pos_control_m + +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_start_nuttx.cpp \ + mc_pos_control.cpp \ + mc_pos_control_params.c diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b..9bc9be245 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/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index f827e70c9..a744d58cf 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -82,7 +82,7 @@ Loiter::on_activation() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092da..a94082d6f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -39,6 +39,7 @@ * @author Thomas Gubler <thomasgubler@gmail.com> * @author Anton Babushkin <anton.babushkin@me.com> * @author Ban Siesta <bansiesta@gmail.com> + * @author Simon Wilks <simon@uaventure.com> */ #include <sys/types.h> @@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), + _param_yawmode(this, "MIS_YAWMODE", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), + _on_arrival_yaw(NAN), _distance_current_previous(0.0f) { /* load initial params */ @@ -166,6 +169,13 @@ Mission::on_active() _navigator->set_can_loiter_at_sp(true); } } + + /* see if we need to update the current yaw heading for rotary wing types */ + if (_navigator->get_vstatus()->is_rotary_wing + && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _mission_type != MISSION_TYPE_NONE) { + heading_sp_update(); + } } void @@ -275,7 +285,7 @@ Mission::check_dist_1wp() &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || @@ -362,7 +372,6 @@ Mission::set_mission_items() mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; - } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { @@ -388,7 +397,7 @@ Mission::set_mission_items() pos_sp_triplet->next.valid = false; /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); set_mission_finished(); @@ -396,6 +405,10 @@ Mission::set_mission_items() return; } + if (pos_sp_triplet->current.valid) { + _on_arrival_yaw = _mission_item.yaw; + } + /* do takeoff on first waypoint for rotary wing vehicles */ if (_navigator->get_vstatus()->is_rotary_wing) { /* force takeoff if landed (additional protection) */ @@ -442,6 +455,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -462,7 +476,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _need_takeoff = true; } @@ -481,7 +495,6 @@ Mission::set_mission_items() if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; @@ -504,6 +517,59 @@ Mission::set_mission_items() } void +Mission::heading_sp_update() +{ + if (_takeoff) { + /* we don't want to be yawing during takeoff */ + return; + } + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_on_arrival_yaw)) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + /* set yaw angle for the waypoint iff a loiter time has been specified */ + if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { + _mission_item.yaw = _on_arrival_yaw; + /* always keep the front of the rotary wing pointing to the next waypoint */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); +} + + +void Mission::altitude_sp_foh_update() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a8a644b0f..e9f78e8fd 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -83,6 +83,13 @@ public: MISSION_ALTMODE_FOH = 1 }; + enum mission_yaw_mode { + MISSION_YAWMODE_NONE = 0, + MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, + MISSION_YAWMODE_FRONT_TO_HOME = 2, + MISSION_YAWMODE_BACK_TO_HOME = 3 + }; + private: /** * Update onboard mission topic @@ -111,6 +118,11 @@ private: void set_mission_items(); /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + + /** * Updates the altitude sp to follow a foh */ void altitude_sp_foh_update(); @@ -155,6 +167,7 @@ private: control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; control::BlockParamInt _param_altmode; + control::BlockParamInt _param_yawmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -177,7 +190,8 @@ private: float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, - can be replaced by a full copy of the previous mission item if needed*/ + can be replaced by a full copy of the previous mission item if needed */ + float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7c..52ccebac9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached() } } + /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ @@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached() } } - /* check if the current waypoint was reached */ + /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { @@ -193,25 +194,25 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite switch (item->nav_cmd) { case NAV_CMD_IDLE: - sp->type = SETPOINT_TYPE_IDLE; + sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; case NAV_CMD_TAKEOFF: - sp->type = SETPOINT_TYPE_TAKEOFF; + sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; break; case NAV_CMD_LAND: - sp->type = SETPOINT_TYPE_LAND; + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: case NAV_CMD_LOITER_TURN_COUNT: case NAV_CMD_LOITER_UNLIMITED: - sp->type = SETPOINT_TYPE_LOITER; + sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; break; default: - sp->type = SETPOINT_TYPE_POSITION; + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; break; } } diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 04c01fe51..6310cf6de 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @group Mission */ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); + +/** + * Multirotor only. Yaw setpoint mode. + * + * 0: Set the yaw heading to the yaw value specified for the destination waypoint. + * 1: Maintain a yaw heading pointing towards the next waypoint. + * 2: Maintain a yaw heading that always points to the home location. + * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). + * + * The values are defined in the enum mission_altitude_mode + * + * @min 0 + * @max 3 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_YAWMODE, 0); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3f7670ec4..e35b0bd6a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -217,7 +217,7 @@ Navigator::vehicle_status_update() { if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { /* in case the commander is not be running */ - _vstatus.arming_state = ARMING_STATE_STANDBY; + _vstatus.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; } } @@ -419,25 +419,25 @@ Navigator::task_main() /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { - case NAVIGATION_STATE_MANUAL: - case NAVIGATION_STATE_ACRO: - case NAVIGATION_STATE_ALTCTL: - case NAVIGATION_STATE_POSCTL: - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_ACRO: + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + case vehicle_status_s::NAVIGATION_STATE_LAND: + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; - case NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; - case NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RCRECOVER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; @@ -445,11 +445,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ _pos_sp_triplet_published_invalid_once = false; @@ -459,11 +459,11 @@ Navigator::task_main() _navigation_mode = &_rtl; } break; - case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; 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 38011a935..ec297e7f0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -53,6 +53,10 @@ #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> @@ -247,9 +251,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -876,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; @@ -906,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 */ @@ -990,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) { @@ -1069,36 +1073,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ - alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp2 = - z_est[0] - alt_avg; - alt_disp2 = alt_disp2 * alt_disp2; - float land_disp2 = params.land_disp * params.land_disp; - /* get actual thrust output */ - float thrust = armed.armed ? actuator.control[3] : 0.0f; - - if (landed) { - if (alt_disp2 > land_disp2 || thrust > params.land_thr) { - landed = false; - landed_time = 0; - } - } else { - if (alt_disp2 < land_disp2 && thrust < params.land_thr) { - if (landed_time == 0) { - landed_time = t; // land detected first time - - } else { - if (t > landed_time + params.land_t * 1000000.0f) { - landed = true; - landed_time = 0; - } - } - - } else { - landed_time = 0; - } - } - if (verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1149,7 +1123,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; 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 b7f6509d7..5387b7e87 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -64,6 +64,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 * * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. @@ -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 d0a65e42e..51bbda412 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/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58c9429b6..d20f776d6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; + if (!(num_values) || !(values) || !(frame_len)) { + return result; + } + /* avoid racing with PPM updates */ irqstate_t state = irqsave(); @@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) if (*num_values > PX4IO_RC_INPUT_CHANNELS) *num_values = PX4IO_RC_INPUT_CHANNELS; - for (unsigned i = 0; i < *num_values; i++) { + for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; } @@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) ppm_last_valid_decode = 0; /* store PPM frame length */ - if (num_values) - *frame_len = ppm_frame_length; + *frame_len = ppm_frame_length; /* good if we got any channels */ result = (*num_values > 0); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d35b70239..8ac87b238 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -70,6 +70,10 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position.h> @@ -94,6 +98,7 @@ #include <uORB/topics/servorail_status.h> #include <uORB/topics/wind_estimate.h> #include <uORB/topics/encoders.h> +#include <uORB/topics/vtol_vehicle_status.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -1000,6 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; struct encoders_s encoders; + struct vtol_vehicle_status_s vtol_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1019,6 +1025,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1053,6 +1060,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int vtol_status_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -1086,12 +1094,13 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -1112,6 +1121,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); @@ -1218,6 +1228,13 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } + /* --- VTOL VEHICLE STATUS --- */ + if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) { + log_msg.msg_type = LOG_VTOL_MSG; + log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; + LOGBUFFER_WRITE_AND_COUNT(VTOL); + } + /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { @@ -1424,6 +1441,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ATTITUDE --- */ if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.q_w = buf.att.q[0]; + log_msg.body.log_ATT.q_x = buf.att.q[1]; + log_msg.body.log_ATT.q_y = buf.att.q[2]; + log_msg.body.log_ATT.q_z = buf.att.q[3]; log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; @@ -1434,18 +1455,6 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); - // secondary attitude - log_msg.msg_type = LOG_ATT2_MSG; - log_msg.body.log_ATT.roll = buf.att.roll_sec; - log_msg.body.log_ATT.pitch = buf.att.pitch_sec; - log_msg.body.log_ATT.yaw = buf.att.yaw_sec; - log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; - log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; - log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; - log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; - log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; - log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; - LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ @@ -1468,7 +1477,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); @@ -1514,7 +1523,6 @@ int sdlog2_thread_main(int argc, char *argv[]) (buf.local_pos.v_z_valid ? 8 : 0) | (buf.local_pos.xy_global ? 16 : 0) | (buf.local_pos.z_global ? 32 : 0); - log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.eph = buf.local_pos.eph; log_msg.body.log_LPOS.epv = buf.local_pos.epv; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5941bfac0..7080d9c31 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,8 +50,11 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 -#define LOG_ATT2_MSG 41 struct log_ATT_s { + float q_w; + float q_x; + float q_y; + float q_z; float roll; float pitch; float yaw; @@ -113,7 +116,6 @@ struct log_LPOS_s { int32_t ref_lon; float ref_alt; uint8_t pos_flags; - uint8_t landed; uint8_t ground_dist_flags; float eph; float epv; @@ -427,6 +429,12 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- VTOL - VTOL VEHICLE STATUS */ +#define LOG_VTOL_MSG 42 +struct log_VTOL_s { + float airspeed_tot; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -455,19 +463,20 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), - LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), + LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a065541b9..67b7feef7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -45,6 +45,13 @@ #include <systemlib/param/param.h> /** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); + +/** * Gyro X-axis offset * * @min -10.0 @@ -98,6 +105,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_MAG_ID, 0); /** * Magnetometer X-axis offset @@ -147,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_ACC_ID, 0); /** * Accelerometer X-axis offset @@ -264,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0a27367d9..867d6c339 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -813,28 +813,28 @@ Sensors::parameters_update() _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th); /* update RC function mappings */ - _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; - _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; - _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; - _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; - - _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; - _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; - - _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; - - _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1; + + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - _rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } /* gyro offsets */ @@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); raw.magnetometer1_raw[0] = mag_report.x_raw; raw.magnetometer1_raw[1] = mag_report.y_raw; @@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); raw.magnetometer2_raw[0] = mag_report.x_raw; raw.magnetometer2_raw[1] = mag_report.y_raw; @@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters @@ -1465,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced) /* this sensor is optional, abort without error */ - if (fd > 0) { + if (fd >= 0) { struct airspeed_scale airscale = { _parameters.diff_pres_offset_pa, 1.0f, @@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced) orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); /* update paramter handles to which the RC channels are mapped */ for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ @@ -1673,17 +1673,17 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else if (mid_inv ? value < mid_th : value > mid_th) { - return SWITCH_POS_MIDDLE; + return manual_control_setpoint_s::SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1694,14 +1694,14 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; if (on_inv ? value < on_th : value > on_th) { - return SWITCH_POS_ON; + return manual_control_setpoint_s::SWITCH_POS_ON; } else { - return SWITCH_POS_OFF; + return manual_control_setpoint_s::SWITCH_POS_OFF; } } else { - return SWITCH_POS_NONE; + return manual_control_setpoint_s::SWITCH_POS_NONE; } } @@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc() { static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) * or no request to map this channel to a param has been sent via mavlink */ continue; } - float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + float rc_val = get_rc_value( (rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { @@ -1847,24 +1847,24 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { @@ -1943,18 +1943,18 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); - _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); - _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); - _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); - _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); - _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); - _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 998b5ac7d..a1a8e7ea8 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -154,6 +154,7 @@ warn(const char *fmt, ...) va_start(args, fmt); vwarn(fmt, args); + va_end(args); } void @@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...) va_start(args, fmt); vwarnc(errcode, fmt, args); + va_end(args); } void @@ -184,6 +186,7 @@ warnx(const char *fmt, ...) va_start(args, fmt); vwarnx(fmt, args); + va_end(args); } void diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 4bcf95784..24f4e4207 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 <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) { diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index 1b3d0aba9..c8a0bf5cd 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -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. * diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 41a866968..eb2d84726 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index fd1ee4dec..8cbe51119 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index fa0594c2e..db47218d9 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index f82586285..12301ce96 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 9385ce253..71ad09130 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,8 +37,7 @@ MODULE_COMMAND = uorb -# XXX probably excessive, 2048 should be sufficient -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 2048 SRCS = uORB.cpp \ objects_common.cpp \ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 293412455..f60aa8d86 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,24 +46,16 @@ #include <drivers/drv_orb_dev.h> #include <drivers/drv_mag.h> -ORB_DEFINE(sensor_mag0, struct mag_report); -ORB_DEFINE(sensor_mag1, struct mag_report); -ORB_DEFINE(sensor_mag2, struct mag_report); +ORB_DEFINE(sensor_mag, struct mag_report); #include <drivers/drv_accel.h> -ORB_DEFINE(sensor_accel0, struct accel_report); -ORB_DEFINE(sensor_accel1, struct accel_report); -ORB_DEFINE(sensor_accel2, struct accel_report); +ORB_DEFINE(sensor_accel, struct accel_report); #include <drivers/drv_gyro.h> -ORB_DEFINE(sensor_gyro0, struct gyro_report); -ORB_DEFINE(sensor_gyro1, struct gyro_report); -ORB_DEFINE(sensor_gyro2, struct gyro_report); +ORB_DEFINE(sensor_gyro, struct gyro_report); #include <drivers/drv_baro.h> -ORB_DEFINE(sensor_baro0, struct baro_report); -ORB_DEFINE(sensor_baro1, struct baro_report); -ORB_DEFINE(sensor_baro2, struct baro_report); +ORB_DEFINE(sensor_baro, struct baro_report); #include <drivers/drv_range_finder.h> ORB_DEFINE(sensor_range_finder, struct range_finder_report); @@ -83,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/vehicle_land_detected.h" +ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); + #include "topics/satellite_info.h" ORB_DEFINE(satellite_info, struct satellite_info_s); @@ -118,8 +113,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); -ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +#include "topics/mc_virtual_rates_setpoint.h" +ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s); +#include "topics/fw_virtual_rates_setpoint.h" +ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -189,22 +186,25 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s); /* actuator controls, as requested by controller */ #include "topics/actuator_controls.h" -ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +#include "topics/actuator_controls_0.h" +ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s); +#include "topics/actuator_controls_1.h" +ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s); +#include "topics/actuator_controls_2.h" +ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s); +#include "topics/actuator_controls_3.h" +ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s); //Virtual control groups, used for VTOL operation -ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); -ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); +#include "topics/actuator_controls_virtual_mc.h" +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s); +#include "topics/actuator_controls_virtual_fw.h" +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs, struct actuator_outputs_s); #include "topics/actuator_direct.h" ORB_DEFINE(actuator_direct, struct actuator_direct_s); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 446140423..c6fbaaed5 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -68,12 +68,6 @@ struct actuator_outputs_s { */ /* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(actuator_outputs_0); -ORB_DECLARE(actuator_outputs_1); -ORB_DECLARE(actuator_outputs_2); -ORB_DECLARE(actuator_outputs_3); +ORB_DECLARE(actuator_outputs); -/* output sets with pre-defined applications */ -#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) - -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h deleted file mode 100644 index cb2262534..000000000 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 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 mission_item_triplet.h - * Definition of the global WGS84 position setpoint uORB topic. - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ -#define TOPIC_MISSION_ITEM_TRIPLET_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -enum SETPOINT_TYPE -{ - SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ - SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ - SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */ -}; - -struct position_setpoint_s -{ - bool valid; /**< true if setpoint is valid */ - enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ - float x; /**< local position setpoint in m in NED */ - float y; /**< local position setpoint in m in NED */ - float z; /**< local position setpoint in m in NED */ - bool position_valid; /**< true if local position setpoint valid */ - float vx; /**< local velocity setpoint in m/s in NED */ - float vy; /**< local velocity setpoint in m/s in NED */ - float vz; /**< local velocity setpoint in m/s in NED */ - bool velocity_valid; /**< true if local velocity setpoint valid */ - double lat; /**< latitude, in deg */ - double lon; /**< longitude, in deg */ - float alt; /**< altitude AMSL, in m */ - float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ - bool yaw_valid; /**< true if yaw setpoint valid */ - float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ - bool yawspeed_valid; /**< true if yawspeed setpoint valid */ - float loiter_radius; /**< loiter radius (only for fixed wing), in m */ - int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ - float a_x; //**< acceleration x setpoint */ - float a_y; //**< acceleration y setpoint */ - float a_z; //**< acceleration z setpoint */ - bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ - bool acceleration_is_force; //*< interprete acceleration as force */ -}; - -/** - * Global position setpoint triplet in WGS84 coordinates. - * - * This are the three next waypoints (or just the next two or one). - */ -struct position_setpoint_triplet_s -{ - struct position_setpoint_s previous; - struct position_setpoint_s current; - struct position_setpoint_s next; - - unsigned nav_state; /**< report the navigation state */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(position_setpoint_triplet); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_land_detected.h index 5dac877d0..51b3568e8 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin <anton.babushkin@me.com> + * 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 @@ -33,12 +32,14 @@ ****************************************************************************/ /** - * @file vehicle_global_velocity_setpoint.h - * Definition of the global velocity setpoint uORB topic. + * @file vehicle_land_detected.h + * Land detected uORB topic + * + * @author Johan Jansen <jnsn.johan@gmail.com> */ -#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#ifndef __TOPIC_VEHICLE_LANDED_H__ +#define __TOPIC_VEHICLE_LANDED_H__ #include "../uORB.h" @@ -47,17 +48,16 @@ * @{ */ -struct vehicle_global_velocity_setpoint_s { - float vx; /**< in m/s NED */ - float vy; /**< in m/s NED */ - float vz; /**< in m/s NED */ -}; /**< Velocity setpoint in NED frame */ +struct vehicle_land_detected_s { + uint64_t timestamp; /**< timestamp of the setpoint */ + bool landed; /**< true if vehicle is currently landed on the ground*/ +}; /** * @} */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_velocity_setpoint); +ORB_DECLARE(vehicle_land_detected); -#endif +#endif //__TOPIC_VEHICLE_LANDED_H__ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h deleted file mode 100644 index 5d39c897d..000000000 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 vehicle_local_position.h - * Definition of the local fused NED position uORB topic. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> - */ - -#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_ -#define TOPIC_VEHICLE_LOCAL_POSITION_H_ - -#include <stdint.h> -#include <stdbool.h> -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Fused local position in NED. - */ -struct vehicle_local_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool xy_valid; /**< true if x and y are valid */ - bool z_valid; /**< true if z is valid */ - bool v_xy_valid; /**< true if vy and vy are valid */ - bool v_z_valid; /**< true if vz is valid */ - /* Position in local NED frame */ - float x; /**< X position in meters in NED earth-fixed frame */ - float y; /**< X position in meters in NED earth-fixed frame */ - float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ - /* Velocity in NED frame */ - float vx; /**< Ground X Speed (Latitude), m/s in NED */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED */ - /* Heading */ - float yaw; - /* Reference position in GPS / WGS84 frame */ - bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ - uint64_t ref_timestamp; /**< Time when reference position was set */ - double ref_lat; /**< Reference point latitude in degrees */ - double ref_lon; /**< Reference point longitude in degrees */ - float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed */ - /* Distance to surface */ - float dist_bottom; /**< Distance to bottom surface (ground) */ - float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ - uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ - bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ - float eph; - float epv; -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_local_position); - -#endif diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 7b4e22bc8..968c2b6bd 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -55,6 +55,7 @@ struct vtol_vehicle_status_s { uint64_t timestamp; /**< Microseconds since system boot */ bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ + float airspeed_tot; /*< Estimated airspeed over control surfaces */ }; /** diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 2fbb05fee..6f021459c 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <systemlib/err.h> #include <nuttx/arch.h> #include <nuttx/wqueue.h> @@ -68,6 +69,7 @@ namespace { +/* internal use only */ static const unsigned orb_maxpath = 64; /* oddly, ERROR is not defined for c++ */ @@ -81,17 +83,30 @@ enum Flavor { PARAM }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; + int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) { unsigned len; - len = snprintf(buf, orb_maxpath, "/%s/%s", - (f == PUBSUB) ? "obj" : "param", - meta->o_name); + unsigned index = 0; + + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); - if (len >= orb_maxpath) + if (len >= orb_maxpath) { return -ENAMETOOLONG; + } return OK; } @@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) class ORBDevNode : public device::CDev { public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); virtual int open(struct file *filp); @@ -126,6 +141,7 @@ private: struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -133,6 +149,7 @@ private: hrt_abstime _last_update; /**< time the object was last updated */ volatile unsigned _generation; /**< object generation count */ pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ SubscriberData *filp_to_sd(struct file *filp) { SubscriberData *sd = (SubscriberData *)(filp->f_priv); @@ -160,13 +177,14 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : CDev(name, path), _meta(meta), _data(nullptr), _last_update(0), _generation(0), - _publisher(0) + _publisher(0), + _priority(priority) { // enable debug() calls _debug_enabled = true; @@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode() { if (_data != nullptr) delete[] _data; + } int @@ -225,12 +244,15 @@ ORBDevNode::open(struct file *filp) /* default to no pending update */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + filp->f_priv = (void *)sd; ret = CDev::open(filp); if (ret != OK) - free(sd); + delete sd; return ret; } @@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) /* track the last generation that the file has seen */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. @@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -556,40 +585,85 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case ORBIOCADVERTISE: { - const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; const char *objname; + const char *devpath; char nodepath[orb_maxpath]; ORBDevNode *node; + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta); + ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != OK) + if (ret != OK) { return ret; + } + + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); + if (devpath == nullptr) { + return -ENOMEM; + } - if (objname == nullptr) - return -ENOMEM; + /* construct the new node */ + node = new ORBDevNode(meta, objname, devpath, adv->priority); - /* construct the new node */ - node = new ORBDevNode(meta, objname, nodepath); + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } - /* initialise the node - this may fail if e.g. a node with this name already exists */ - if (node != nullptr) + /* initialise the node - this may fail if e.g. a node with this name already exists */ ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + free((void *)devpath); + } - /* if we didn't get a device, that's bad */ - if (node == nullptr) - return -ENOMEM; + group_tries++; - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); + } while (ret != OK && (group_tries < max_group_tries)); + + if (group_tries > max_group_tries) { + ret = -ENOMEM; } + /* the file handle for the driver has been created, unlock */ + unlock(); + return ret; } @@ -614,6 +688,7 @@ struct orb_test { }; ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); int test_fail(const char *fmt, ...) @@ -643,8 +718,6 @@ test_note(const char *fmt, ...) return OK; } -ORB_DECLARE(sensor_combined); - int test() { @@ -700,48 +773,65 @@ test() orb_unsubscribe(sfd); close(pfd); -#if 0 - /* this is a hacky test that exploits the sensors app to test rate-limiting */ + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); - sfd = orb_subscribe(ORB_ID(sensor_combined)); + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - hrt_abstime start, end; - unsigned count; + test_note("advertised"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - do { - orb_check(sfd, &updated); + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); - end = hrt_absolute_time(); - test_note("full-speed, 100 updates in %llu", end - start); + t.val = 103; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); - orb_set_interval(sfd, 10); + test_note("published"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + t.val = 203; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); - do { - orb_check(sfd, &updated); + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + return test_fail("sub #0 copy failed: %d", errno); - end = hrt_absolute_time(); - test_note("100Hz, 100 updates in %llu", end - start); + if (t.val != 103) + return test_fail("sub #0 val. mismatch: %d", t.val); - orb_unsubscribe(sfd); -#endif + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + return test_fail("sub #1 copy failed: %d", errno); + + if (t.val != 203) + return test_fail("sub #1 val. mismatch: %d", t.val); + + /* test priorities */ + int prio; + if (OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); return test_note("PASS"); } @@ -771,7 +861,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) { - fprintf(stderr, "[uorb] already loaded\n"); + warnx("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; } @@ -780,18 +870,17 @@ uorb_main(int argc, char *argv[]) g_dev = new ORBDevMaster(PUBSUB); if (g_dev == nullptr) { - fprintf(stderr, "[uorb] driver alloc failed\n"); + warnx("driver alloc failed"); return -ENOMEM; } if (OK != g_dev->init()) { - fprintf(stderr, "[uorb] driver init failed\n"); + warnx("driver init failed"); delete g_dev; g_dev = nullptr; return -EIO; } - printf("[uorb] ready\n"); return OK; } @@ -807,8 +896,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); - return -EINVAL; + errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); } /* @@ -825,11 +913,14 @@ namespace * we tried to advertise. */ int -node_advertise(const struct orb_metadata *meta) +node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { int fd = -1; int ret = ERROR; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + /* open the control device */ fd = open(TOPIC_MASTER_DEVICE_PATH, 0); @@ -837,11 +928,12 @@ node_advertise(const struct orb_metadata *meta) goto out; /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) + if ((OK != ret) && (EEXIST == errno)) { ret = OK; + } out: @@ -858,7 +950,7 @@ out: * advertisers. */ int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { char path[orb_maxpath]; int fd, ret; @@ -883,7 +975,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ - ret = node_mkpath(path, f, meta); + ret = node_mkpath(path, f, meta, instance); if (ret != OK) { errno = -ret; @@ -893,15 +985,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* open the path as either the advertiser or the subscriber */ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + /* we may need to advertise the node... */ if (fd < 0) { /* try to create the node */ - ret = node_advertise(meta); + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } /* on success, try the open again */ - if (ret == OK) + if (ret == OK) { fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } } if (fd < 0) { @@ -918,11 +1029,17 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t +orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) +{ int result, fd; orb_advert_t advertiser; /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true); + fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) return ERROR; @@ -933,7 +1050,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data) return ERROR; /* the advertiser must perform an initial publish to initialise the object */ - result= orb_publish(meta, advertiser, data); + result = orb_publish(meta, advertiser, data); if (result == ERROR) return ERROR; @@ -947,6 +1064,13 @@ orb_subscribe(const struct orb_metadata *meta) } int +orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + +int orb_unsubscribe(int handle) { return close(handle); @@ -989,6 +1113,12 @@ orb_stat(int handle, uint64_t *time) } int +orb_priority(int handle, int *priority) +{ + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + +int orb_set_interval(int handle, unsigned interval) { return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index f19fbba7c..b54f0322b 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,27 +57,23 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; /** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. + * Maximum number of multi topic instances */ -#define ORB_ID(_name) &__orb_##_name +#define ORB_MULTI_MAX_INSTANCES 3 /** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic + * Topic priority. + * Relevant for multi-topics / topic groups */ -#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) +enum ORB_PRIO { + ORB_PRIO_MIN = 0, + ORB_PRIO_VERY_LOW = 25, + ORB_PRIO_LOW = 50, + ORB_PRIO_DEFAULT = 75, + ORB_PRIO_HIGH = 100, + ORB_PRIO_VERY_HIGH = 125, + ORB_PRIO_MAX = 255 +}; /** * Generates a pointer to the uORB metadata structure for @@ -87,12 +83,8 @@ typedef const struct orb_metadata *orb_id_t; * with ORB_DECLARE(). * * @param _name The name of the topic. - * @param _count The class ID of the topic */ -#define ORB_ID_TRIPLE(_name, _count) \ - ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ - ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ - (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) +#define ORB_ID(_name) &__orb_##_name /** * Declare (prototype) the uORB metadata for a topic. @@ -168,6 +160,34 @@ typedef intptr_t orb_advert_t; extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; /** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; + + +/** * Publish new data to a topic. * * The data is atomically published to the topic and any waiting subscribers @@ -211,6 +231,37 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; /** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; + +/** * Unsubscribe from a topic. * * @param handle A handle returned from orb_subscribe. @@ -267,6 +318,18 @@ extern int orb_check(int handle, bool *updated) __EXPORT; extern int orb_stat(int handle, uint64_t *time) __EXPORT; /** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_priority(int handle, int *priority) __EXPORT; + +/** * Set the minimum interval between which updates are seen for a subscription. * * If this interval is set, the subscriber will not see more than one update @@ -290,25 +353,6 @@ __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) -ORB_DECLARE(actuator_controls_0); -typedef struct actuator_controls_s actuator_controls_0_s; -ORB_DECLARE(actuator_controls_1); -typedef struct actuator_controls_s actuator_controls_1_s; -ORB_DECLARE(actuator_controls_2); -typedef struct actuator_controls_s actuator_controls_2_s; -ORB_DECLARE(actuator_controls_3); -typedef struct actuator_controls_s actuator_controls_3_s; -ORB_DECLARE(actuator_controls_virtual_mc); -typedef struct actuator_controls_s actuator_controls_virtual_mc_s; -ORB_DECLARE(actuator_controls_virtual_fw); -typedef struct actuator_controls_s actuator_controls_virtual_fw_s; -ORB_DECLARE(mc_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s mc_virtual_rates_setpoint_s; -ORB_DECLARE(fw_virtual_rates_setpoint); -typedef struct vehicle_rates_setpoint_s fw_virtual_rates_setpoint_s; -ORB_DECLARE(mc_virtual_attitude_setpoint); -typedef struct vehicle_attitude_setpoint_s mc_virtual_attitude_setpoint_s; -ORB_DECLARE(fw_virtual_attitude_setpoint); typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s; typedef uint8_t arming_state_t; typedef uint8_t main_state_t; diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e1..51589e43e 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,6 +49,13 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } } UavcanEscController::~UavcanEscController() diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c4..4f63629a0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. # Author: Pavel Kirienko <pavel.kirienko@gmail.com> # # Redistribution and use in source and binary forms, with or without @@ -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/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8741ae20d..ad09dfcac 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 @@ -38,15 +38,10 @@ #include "baro.hpp" #include <cmath> -static const orb_id_t BARO_TOPICS[2] = { - ORB_ID(sensor_baro0), - ORB_ID(sensor_baro1) -}; - const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_data(node) { } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 9d470219e..c7bbc5af8 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 571a6f1cd..3ae07367f 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 2111ff80b..96ff9404f 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 35ebee542..ee278aaf5 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 @@ -39,16 +39,10 @@ #include <systemlib/err.h> -static const orb_id_t MAG_TOPICS[3] = { - ORB_ID(sensor_mag0), - ORB_ID(sensor_mag1), - ORB_ID(sensor_mag2) -}; - const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 74077d883..db38aee1d 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 0999938fc..b37076444 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } - delete [] _orb_topics; delete [] _channels; } @@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; channel->node_id = node_id; channel->class_instance = class_instance; - channel->orb_advert = orb_advertise(channel->orb_id, report); + channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } assert(channel != nullptr); - (void)orb_publish(channel->orb_id, channel->orb_advert, report); + (void)orb_publish(_orb_topic, channel->orb_advert, report); } unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index e31960537..de130b078 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 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 @@ -75,8 +75,7 @@ public: /** * Sensor bridge factory. - * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". - * @return nullptr if such bridge can't be created. + * Creates all known sensor bridges and puts them in the linked list. */ static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list); }; @@ -90,28 +89,29 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD struct Channel { int node_id = -1; - orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; + int orb_instance = -1; }; const unsigned _max_channels; const char *const _class_devname; - orb_id_t *const _orb_topics; + const orb_id_t _orb_topic; Channel *const _channels; bool _out_of_channels = false; protected: - template <unsigned MaxChannels> + static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t (&orb_topics)[MaxChannels]) : + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : device::CDev(name, devname), - _max_channels(MaxChannels), + _max_channels(max_channels), _class_devname(class_devname), - _orb_topics(new orb_id_t[MaxChannels]), - _channels(new Channel[MaxChannels]) + _orb_topic(orb_topic_sensor), + _channels(new Channel[max_channels]) { - memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; } diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c7..b93a95f96 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 @@ -359,7 +381,7 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; @@ -371,14 +393,14 @@ int UavcanNode::run() /* see if we have any direct actuator updates */ - if (_actuator_direct_sub != -1 && + if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && !_test_in_progress) { if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; } - memcpy(&_outputs.output[0], &_actuator_direct.values[0], + memcpy(&_outputs.output[0], &_actuator_direct.values[0], _actuator_direct.nvalues*sizeof(float)); _outputs.noutputs = _actuator_direct.nvalues; new_output = true; @@ -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); } @@ -452,7 +476,7 @@ int UavcanNode::run() if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down and motor + // Update the armed status and check that we're not locked down and motor // test is not running bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; @@ -478,13 +502,13 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co int UavcanNode::teardown() { - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } } - return ::close(_armed_sub); + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } int @@ -502,7 +526,7 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); _control_subs[i] = orb_subscribe(_control_topics[i]); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 98f2e5ad4..19b9b4b48 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include <nuttx/config.h> - #include <uavcan_stm32/uavcan_stm32.hpp> #include <drivers/device/device.h> +#include <systemlib/perf_counter.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -66,7 +66,7 @@ */ class UavcanNode : public device::CDev { - static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why static constexpr unsigned RxQueueLenPerIface = 64; static constexpr unsigned StackSize = 3000; @@ -107,11 +107,11 @@ private: int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver int _armed_sub = -1; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system + actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus int _test_motor_sub = -1; ///< uORB subscription of the test_motor status - test_motor_s _test_motor; + test_motor_s _test_motor = {}; bool _test_in_progress = false; unsigned _output_count = 0; ///< number of actuators currently available @@ -135,11 +135,15 @@ private: unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic - uint8_t _actuator_direct_poll_fd_num; - actuator_direct_s _actuator_direct; + uint8_t _actuator_direct_poll_fd_num = 0; + actuator_direct_s _actuator_direct = {}; - actuator_outputs_s _outputs; + actuator_outputs_s _outputs = {}; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + + perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); + perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); + perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); }; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c72a85ecd..0a333eade 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -58,13 +58,19 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_virtual_mc.h> +#include <uORB/topics/actuator_controls_virtual_fw.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/mc_virtual_rates_setpoint.h> +#include <uORB/topics/fw_virtual_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/battery_status.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> @@ -105,7 +111,9 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -129,7 +137,9 @@ private: struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status struct { param_t idle_pwm_mc; //pwm value for idle in mc mode @@ -139,6 +149,9 @@ private: float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain } _params; struct { @@ -149,6 +162,9 @@ private: param_t mc_airspeed_trim; param_t mc_airspeed_max; param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -159,6 +175,7 @@ private: * to waste energy when gliding. */ bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" unsigned _motor_count; // number of motors + float _airspeed_tot; //*****************Member functions*********************************************************************** @@ -172,7 +189,9 @@ private: void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates void parameters_update_poll(); //Check if parameters have changed int parameters_update(); //Update local paraemter cache void fill_mc_att_control_output(); //write mc_att_control results to actuator message @@ -182,6 +201,7 @@ private: void set_idle_fw(); void set_idle_mc(); void scale_mc_output(); + void calc_tot_airspeed(); // estimated airspeed seen by elevons }; namespace VTOL_att_control @@ -205,7 +225,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _local_pos_sub(-1), _airspeed_sub(-1), + _battery_status_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -218,6 +240,7 @@ VtolAttitudeControl::VtolAttitudeControl() : { flag_idle_mc = true; + _airspeed_tot = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -234,7 +257,9 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); + memset(&_local_pos,0,sizeof(_local_pos)); memset(&_airspeed,0,sizeof(_airspeed)); + memset(&_batt_status,0,sizeof(_batt_status)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -247,6 +272,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); + _params_handles.power_max = param_find("VT_POWER_MAX"); + _params_handles.prop_eff = param_find("VT_PROP_EFF"); + _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); /* fetch initial parameter values */ parameters_update(); @@ -388,6 +416,19 @@ VtolAttitudeControl::vehicle_airspeed_poll() { } /** +* Check for battery updates. +*/ +void +VtolAttitudeControl::vehicle_battery_poll() { + bool updated; + orb_check(_battery_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status); + } +} + +/** * Check for parameter updates. */ void @@ -406,6 +447,22 @@ VtolAttitudeControl::parameters_update_poll() } /** +* Check for sensor updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos); + } + +} + +/** * Update parameters. */ int @@ -437,6 +494,18 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_pitch_trim, &v); _params.fw_pitch_trim = v; + /* vtol maximum power engine can produce */ + param_get(_params_handles.power_max, &v); + _params.power_max = v; + + /* vtol propeller efficiency factor */ + param_get(_params_handles.prop_eff, &v); + _params.prop_eff = v; + + /* vtol total airspeed estimate low pass gain */ + param_get(_params_handles.arsp_lp_gain, &v); + _params.arsp_lp_gain = v; + return OK; } @@ -555,7 +624,7 @@ void VtolAttitudeControl::scale_mc_output() { // scale around tuning airspeed float airspeed; - + calc_tot_airspeed(); // estimate air velocity seen by elevons // if airspeed is not updating, we assume the normal average speed if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { @@ -563,16 +632,12 @@ VtolAttitudeControl::scale_mc_output() { if (nonfinite) { perf_count(_nonfinite_input_perf); } - } else { - // prevent numerical drama by requiring 0.5 m/s minimal speed - airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); - } - - // Sanity check if airspeed is consistent with throttle - if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param - airspeed = _params.mc_airspeed_trim; + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); } + _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we @@ -584,6 +649,23 @@ VtolAttitudeControl::scale_mc_output() { _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); } +void VtolAttitudeControl::calc_tot_airspeed() { + float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; + P = math::constrain(P,1.0f,_params.power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -604,7 +686,9 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -674,7 +758,10 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); + vehicle_battery_poll(); + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -688,7 +775,8 @@ void VtolAttitudeControl::task_main() if (fds[0].revents & POLLIN) { vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with airspeed + + // scale pitch control with total airspeed scale_mc_output(); fill_mc_att_control_output(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index d1d4697f3..33752b2c4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); /** * Maximum airspeed in multicopter mode @@ -109,3 +109,36 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); */ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +/** + * Motor max power + * + * Indicates the maximum power the motor is able to produce. Used to calculate + * propeller efficiency map. + * + * @min 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); + +/** + * Propeller efficiency parameter + * + * Influences propeller efficiency at different power settings. Should be tuned beforehand. + * + * @min 0.5 + * @max 0.9 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); + +/** + * Total airspeed estimate low-pass filter gain + * + * Gain for tuning the low-pass filter for the total airspeed estimate + * + * @min 0.0 + * @max 0.99 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); + diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp deleted file mode 100644 index 426e646c9..000000000 --- a/src/platforms/nuttx/px4_subscriber.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file px4_subscriber.cpp - * - * PX4 Middleware Wrapper Subscriber - */ -#include <platforms/px4_subscriber.h> - diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6b6a03893..fa4e1398e 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -50,7 +50,6 @@ /* * Building for running within the ROS environment */ -#define __EXPORT #define noreturn_function #ifdef __cplusplus #include "ros/ros.h" @@ -136,15 +135,14 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */ #define M_LOG2_E_F _M_LN2_F #define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */ +#define M_DEG_TO_RAD 0.01745329251994 +#define M_RAD_TO_DEG 57.2957795130823 #else /* * Building for NuttX */ #include <platforms/px4_includes.h> -#ifdef __cplusplus -#include <functional> -#endif /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index fc31162c8..f8561fa3b 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -48,20 +48,24 @@ #ifdef __cplusplus #include "ros/ros.h" -#include "px4/rc_channels.h" -#include "px4/vehicle_attitude.h" -#include <px4/vehicle_attitude_setpoint.h> -#include <px4/manual_control_setpoint.h> -#include <px4/actuator_controls.h> -#include <px4/actuator_controls_0.h> -#include <px4/actuator_controls_virtual_mc.h> -#include <px4/vehicle_rates_setpoint.h> -#include <px4/mc_virtual_rates_setpoint.h> -#include <px4/vehicle_attitude.h> -#include <px4/vehicle_control_mode.h> -#include <px4/actuator_armed.h> -#include <px4/parameter_update.h> -#include <px4/vehicle_status.h> +#include <px4_rc_channels.h> +#include <px4_vehicle_attitude.h> +#include <px4_vehicle_attitude_setpoint.h> +#include <px4_manual_control_setpoint.h> +#include <px4_actuator_controls.h> +#include <px4_actuator_controls_0.h> +#include <px4_actuator_controls_virtual_mc.h> +#include <px4_vehicle_rates_setpoint.h> +#include <px4_mc_virtual_rates_setpoint.h> +#include <px4_vehicle_attitude.h> +#include <px4_vehicle_control_mode.h> +#include <px4_actuator_armed.h> +#include <px4_parameter_update.h> +#include <px4_vehicle_status.h> +#include <px4_vehicle_local_position_setpoint.h> +#include <px4_vehicle_global_velocity_setpoint.h> +#include <px4_vehicle_local_position.h> +#include <px4_position_setpoint_triplet.h> #endif #else @@ -70,16 +74,26 @@ */ #include <nuttx/config.h> #include <uORB/uORB.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_status.h> +#ifdef __cplusplus +#include <platforms/nuttx/px4_messages/px4_rc_channels.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h> +#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h> +#include <platforms/nuttx/px4_messages/px4_actuator_armed.h> +#include <platforms/nuttx/px4_messages/px4_parameter_update.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_status.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_local_position_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h> +#include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h> +#endif #include <systemlib/err.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> diff --git a/src/platforms/ros/px4_subscriber.cpp b/src/platforms/px4_message.h index d040b860d..bff7aa313 100644 --- a/src/platforms/ros/px4_subscriber.cpp +++ b/src/platforms/px4_message.h @@ -32,10 +32,46 @@ ****************************************************************************/ /** - * @file px4_subscriber.cpp + * @file px4_message.h * - * PX4 Middleware Wrapper Subscriber + * Defines the message base types */ +#pragma once -#include <px4_subscriber.h> +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include <uORB/uORB.h> +typedef orb_id_t PX4TopicHandle; +#endif +namespace px4 +{ + +template <typename M> +class __EXPORT PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template<typename T> + // friend class SubscriberROS; +// #endif + +public: + PX4Message() : + _data() + {} + + PX4Message(M msg) : + _data(msg) + {} + + virtual ~PX4Message() {}; + + virtual M& data() {return _data;} + virtual const M& data() const {return _data;} +private: + M _data; +}; + +} diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index cd52fd650..735d34234 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -42,10 +42,6 @@ #include <stdint.h> #include <unistd.h> -#if defined(__PX4_ROS) -#define __EXPORT -#endif - namespace px4 { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 7b14caed9..83a3e422d 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,10 +48,12 @@ #include "ros/ros.h" #include <list> #include <inttypes.h> +#include <type_traits> #else /* includes when building for NuttX */ #include <poll.h> #endif +#include <functional> namespace px4 { @@ -77,59 +79,47 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template<typename M> - Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &)) + template<typename T> + Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval) { - SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); - return (Subscriber<M> *)sub; + return (Subscriber<T> *)sub; } /** * Subscribe with callback to class method - * @param topic Name of the topic * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance */ - template<typename M, typename T> - Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) + template<typename T, typename C> + Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { - SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); - return (Subscriber<M> *)sub; + return (Subscriber<T> *)sub; } /** * Subscribe with no callback, just the latest value is stored on updates - * @param topic Name of the topic */ - template<typename M> - Subscriber<M> *subscribe(const char *topic) + template<typename T> + Subscriber<T> *subscribe(unsigned interval) { - SubscriberBase *sub = new SubscriberROS<M>(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this); _subs.push_back(sub); - return (Subscriber<M> *)sub; + return (Subscriber<T> *)sub; } /** * Advertise topic - * @param topic Name of the topic */ - template<typename M> - Publisher *advertise(const char *topic) + template<typename T> + Publisher<T>* advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); - Publisher *pub = new Publisher(ros_pub); - _pubs.push_back(pub); - return pub; + PublisherROS<T> *pub = new PublisherROS<T>((ros::NodeHandle*)this); + _pubs.push_back((PublisherBase*)pub); + return (Publisher<T>*)pub; } /** @@ -143,8 +133,7 @@ public: void spin() { ros::spin(); } -private: - static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ +protected: std::list<SubscriberBase *> _subs; /**< Subcriptions of node */ std::list<PublisherBase *> _pubs; /**< Publications of node */ }; @@ -161,7 +150,7 @@ public: ~NodeHandle() { /* Empty subscriptions list */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -170,13 +159,13 @@ public: break; } - uORB::SubscriptionNode *sib = sub->getSibling(); + SubscriberNode *sib = sub->getSibling(); delete sub; sub = sib; } /* Empty publications list */ - uORB::PublicationNode *pub = _pubs.getHead(); + PublisherNode *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +174,7 @@ public: break; } - uORB::PublicationNode *sib = pub->getSibling(); + PublisherNode *sib = pub->getSibling(); delete pub; pub = sib; } @@ -193,56 +182,59 @@ public: /** * Subscribe with callback to function - * @param meta Describes the topic which nodehande should subscribe to - * @param callback Callback, executed on receiving a new message + * @param fp Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ - template<typename M> - Subscriber<M> *subscribe(const struct orb_metadata *meta, - std::function<void(const M &)> callback, - unsigned interval) + template<typename T> + Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval) { - SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - _sub_min_interval = sub_px4; - } + (void)interval; + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1)); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; + } - return (Subscriber<M> *)sub_px4; + /** + * Subscribe with callback to class method + * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance + */ + template<typename T, typename C> + Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) + { + (void)interval; + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1)); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; } /** * Subscribe without callback to function - * @param meta Describes the topic which nodehande should subscribe to * @param interval Minimal interval between data fetches from orb */ - template<typename M> - Subscriber<M> *subscribe(const struct orb_metadata *meta, - unsigned interval) + template<typename T> + Subscriber<T> *subscribe(unsigned interval) { - SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs); - - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { - _sub_min_interval = sub_px4; - } - - return (Subscriber<M> *)sub_px4; + (void)interval; + SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval); + update_sub_min_interval(interval, sub_px4); + _subs.add((SubscriberNode *)sub_px4); + return (Subscriber<T> *)sub_px4; } /** * Advertise topic - * @param meta Describes the topic which is advertised */ - template<typename M> - Publisher *advertise(const struct orb_metadata *meta) + template<typename T> + Publisher<T> *advertise() { - //XXX - Publisher *pub = new Publisher(meta, &_pubs); - return pub; + PublisherUORB<T> *pub = new PublisherUORB<T>(); + _pubs.add(pub); + return (Publisher<T>*)pub; } /** @@ -251,7 +243,7 @@ public: void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -281,19 +273,30 @@ public: /* Poll fd with smallest interval */ struct pollfd pfd; - pfd.fd = _sub_min_interval->getHandle(); + pfd.fd = _sub_min_interval->getUORBHandle(); pfd.events = POLLIN; poll(&pfd, 1, timeout_ms); spinOnce(); } } -private: +protected: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; - List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */ - List<uORB::PublicationNode *> _pubs; /**< Publications of node */ - uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + List<SubscriberNode *> _subs; /**< Subcriptions of node */ + List<PublisherNode *> _pubs; /**< Publications of node */ + SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ + + /** + * Check if this is the smallest interval so far and update _sub_min_interval + */ + template<typename T> + void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub) + { + if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + _sub_min_interval = sub; + } + } }; #endif } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index c6f3d6108..d9cd7a3c1 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -46,70 +46,106 @@ #include <containers/List.hpp> #endif +#include <platforms/px4_message.h> + namespace px4 { /** * Untemplated publisher base class * */ -class PublisherBase +class __EXPORT PublisherBase { public: PublisherBase() {}; ~PublisherBase() {}; +}; +/** + * Publisher base class, templated with the message type + * */ +template <typename T> +class __EXPORT Publisher +{ +public: + Publisher() {}; + ~Publisher() {}; + + virtual int publish(const T &msg) = 0; }; #if defined(__PX4_ROS) -class Publisher : - public PublisherBase +template <typename T> +class PublisherROS : + public Publisher<T> { public: /** * Construct Publisher by providing a ros::Publisher * @param ros_pub the ros publisher which will be used to perform the publications */ - Publisher(ros::Publisher ros_pub) : - PublisherBase(), - _ros_pub(ros_pub) + PublisherROS(ros::NodeHandle *rnh) : + Publisher<T>(), + _ros_pub(rnh->advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault)) {} - ~Publisher() {}; + ~PublisherROS() {}; /** Publishes msg * @param msg the message which is published to the topic */ - template<typename M> - int publish(const M &msg) { _ros_pub.publish(msg); return 0; } -private: + int publish(const T &msg) + { + _ros_pub.publish(msg.data()); + return 0; + } +protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else -class Publisher : - public uORB::PublicationNode, - public PublisherBase +/** + * Because we maintain a list of publishers we need a node class + */ +class __EXPORT PublisherNode : + public ListNode<PublisherNode *> +{ +public: + PublisherNode() : + ListNode() + {} + + virtual ~PublisherNode() {} + + virtual void update() = 0; +}; + +template <typename T> +class __EXPORT PublisherUORB : + public Publisher<T>, + public PublisherNode + { public: /** * Construct Publisher by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param list publisher is added to this list */ - Publisher(const struct orb_metadata *meta, - List<uORB::PublicationNode *> *list) : - uORB::PublicationNode(meta, list), - PublisherBase() + PublisherUORB() : + Publisher<T>(), + PublisherNode(), + _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~Publisher() {}; + ~PublisherUORB() { + delete _uorb_pub; + }; /** Publishes msg * @param msg the message which is published to the topic */ - template<typename M> - int publish(const M &msg) + int publish(const T &msg) { - uORB::PublicationBase::update((void *)&msg); + _uorb_pub->update((void *)&(msg.data())); return 0; } @@ -117,6 +153,9 @@ public: * Empty callback for list traversal */ void update() {} ; +private: + uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index aaacf9e48..6ca35b173 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -39,6 +39,7 @@ #pragma once #include <functional> +#include <type_traits> #if defined(__PX4_ROS) /* includes when building for ros */ @@ -56,139 +57,147 @@ namespace px4 /** * Untemplated subscriber base class * */ -class SubscriberBase +class __EXPORT SubscriberBase { public: SubscriberBase() {}; - ~SubscriberBase() {}; + virtual ~SubscriberBase() {}; }; /** * Subscriber class which is used by nodehandle */ -template<typename M> -class Subscriber : +template<typename T> +class __EXPORT Subscriber : public SubscriberBase { public: Subscriber() : - SubscriberBase() + SubscriberBase(), + _msg_current() {}; - ~Subscriber() {}; + + virtual ~Subscriber() {} /* Accessors*/ /** * Get the last message value */ - virtual M get() = 0; + virtual T& get() {return _msg_current;} /** - * Get void pointer to last message value + * Get the last native message value */ - virtual void *get_void_ptr() = 0; + virtual decltype(((T*)nullptr)->data()) data() + { + return _msg_current.data(); + } + +protected: + T _msg_current; /**< Current Message value */ }; #if defined(__PX4_ROS) /** * Subscriber class that is templated with the ros n message type */ -template<typename M> +template<typename T> class SubscriberROS : - public Subscriber<M> + public Subscriber<T> { - friend class NodeHandle; - public: /** - * Construct Subscriber by providing a callback function - */ - SubscriberROS(std::function<void(const M &)> cbf) : - Subscriber<M>(), - _ros_sub(), - _cbf(cbf), - _msg_current() - {} - - /** * Construct Subscriber without a callback function */ - SubscriberROS() : - Subscriber<M>(), - _ros_sub(), + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber<T>(), _cbf(NULL), - _msg_current() + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) {} - - ~SubscriberROS() {}; - - /* Accessors*/ /** - * Get the last message value - */ - M get() { return _msg_current; } - /** - * Get void pointer to last message value + * Construct Subscriber by providing a callback function */ - void *get_void_ptr() { return (void *)&_msg_current; } + SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) + {} + + virtual ~SubscriberROS() {}; protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + /** * Called on topic update, saves the current message and then calls the provided callback function + * needs to use the native type as it is called by ROS */ - void callback(const M &msg) + void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) { /* Store data */ - _msg_current = msg; + this->_msg_current.data() = msg; /* Call callback */ if (_cbf != NULL) { - _cbf(msg); + _cbf(this->get()); } } - /** - * Saves the ros subscriber to keep ros subscription alive - */ - void set_ros_sub(ros::Subscriber ros_sub) - { - _ros_sub = ros_sub; - } - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function<void(const M &)> _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; #else // Building for NuttX +/** + * Because we maintain a list of subscribers we need a node class + */ +class __EXPORT SubscriberNode : + public ListNode<SubscriberNode *> +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; /** * Subscriber class that is templated with the uorb subscription message type */ -template<typename M> -class SubscriberUORB : - public Subscriber<M>, - public uORB::Subscription<M> +template<typename T> +class __EXPORT SubscriberUORB : + public Subscriber<T>, + public SubscriberNode { public: /** * Construct SubscriberUORB by providing orb meta data without callback - * @param meta orb metadata for the topic which is used * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List<uORB::SubscriptionNode *> *list) : - Subscriber<M>(), - uORB::Subscription<M>(meta, interval, list) + SubscriberUORB(unsigned interval) : + SubscriberNode(interval), + _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - ~SubscriberUORB() {}; + virtual ~SubscriberUORB() { + delete _uorb_sub; + }; /** * Update Subscription @@ -196,47 +205,50 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { - /* Topic not updated */ + if (!_uorb_sub->updated()) { + /* Topic not updated, do not call callback */ return; } - /* get latest data */ - uORB::Subscription<M>::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ - /** - * Get the last message value - */ - M get() { return uORB::Subscription<M>::getData(); } + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + + typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() + { + return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; + } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + }; -template<typename M> -class SubscriberUORBCallback : - public SubscriberUORB<M> +//XXX reduce to one class with overloaded constructor? +template<typename T> +class __EXPORT SubscriberUORBCallback : + public SubscriberUORB<T> { public: /** * Construct SubscriberUORBCallback by providing orb meta data - * @param meta orb metadata for the topic which is used - * @param callback Callback, executed on receiving a new message + * @param cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function<void(const M &)> callback, - List<uORB::SubscriptionNode *> *list) : - SubscriberUORB<M>(meta, interval, list), - _callback(callback) + SubscriberUORBCallback(unsigned interval, + std::function<void(const T &)> cbf) : + SubscriberUORB<T>(interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -245,28 +257,27 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription<M>::update(); + this->_uorb_sub->update(this->get_void_ptr()); /* Check if there is a callback */ - if (_callback == nullptr) { + if (_cbf == nullptr) { return; } /* Call callback which performs actions based on this data */ - _callback(uORB::Subscription<M>::getData()); + _cbf(Subscriber<T>::get()); }; protected: - std::function<void(const M &)> _callback; /**< Callback handle, - called when new data is available */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; #endif diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 6fad681c9..04094de8b 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -171,3 +171,96 @@ __EXPORT float _wrap_360(float bearing) return bearing; } + +__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref) +{ + return ref->init_done; +} + +__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref) +{ + return ref->timestamp; +} + +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + + ref->lat_rad = lat_0 * M_DEG_TO_RAD; + ref->lon_rad = lon_0 * M_DEG_TO_RAD; + ref->sin_lat = sin(ref->lat_rad); + ref->cos_lat = cos(ref->lat_rad); + + ref->timestamp = timestamp; + ref->init_done = true; + + return 0; +} + +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); +} + +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + *ref_lat_rad = ref->lat_rad; + *ref_lon_rad = ref->lon_rad; + + return 0; +} + +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double lat_rad = lat * M_DEG_TO_RAD; + double lon_rad = lon * M_DEG_TO_RAD; + + double sin_lat = sin(lat_rad); + double cos_lat = cos(lat_rad); + double cos_d_lon = cos(lon_rad - ref->lon_rad); + + double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon); + double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c)); + + *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH; + *y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH; + + return 0; +} + +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +{ + if (!map_projection_initialized(ref)) { + return -1; + } + + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double c = sqrtf(x_rad * x_rad + y_rad * y_rad); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_rad; + double lon_rad; + + if (fabs(c) > DBL_EPSILON) { + lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c); + lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c)); + + } else { + lat_rad = ref->lat_rad; + lon_rad = ref->lon_rad; + } + + *lat = lat_rad * 180.0 / M_PI; + *lon = lon_rad * 180.0 / M_PI; + + return 0; +} diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp index bcee0b479..1d36e3d99 100644 --- a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -43,6 +43,7 @@ #include <px4/vehicle_attitude.h> #include <mathlib/mathlib.h> #include <platforms/px4_defines.h> +#include <platforms/px4_middleware.h> AttitudeEstimator::AttitudeEstimator() : _n(), @@ -92,6 +93,7 @@ void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstP // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } @@ -134,6 +136,7 @@ void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + msg_v_att.timestamp = px4::get_time_micros(); _vehicle_attitude_pub.publish(msg_v_att); } diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index b9fc296f9..2673122c7 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -40,9 +40,6 @@ #include "commander.h" -#include <px4/manual_control_setpoint.h> -#include <px4/vehicle_control_mode.h> -#include <px4/vehicle_status.h> #include <platforms/px4_middleware.h> Commander::Commander() : @@ -53,21 +50,18 @@ Commander::Commander() : _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), _msg_parameter_update(), - _msg_actuator_armed() + _msg_actuator_armed(), + _msg_vehicle_control_mode() { } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { - px4::vehicle_control_mode msg_vehicle_control_mode; px4::vehicle_status msg_vehicle_status; - /* fill vehicle control mode */ - //XXX hardcoded - msg_vehicle_control_mode.timestamp = px4::get_time_micros(); - msg_vehicle_control_mode.flag_control_manual_enabled = true; - msg_vehicle_control_mode.flag_control_rates_enabled = true; - msg_vehicle_control_mode.flag_control_attitude_enabled = true; + /* fill vehicle control mode based on (faked) stick positions*/ + EvalSwitches(msg, msg_vehicle_status, _msg_vehicle_control_mode); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); /* fill actuator armed */ float arm_th = 0.95; @@ -77,26 +71,26 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon /* Check for disarm */ if (msg->r < -arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = false; + _msg_vehicle_control_mode.flag_armed = false; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_STANDBY; } } else { /* Check for arm */ if (msg->r > arm_th && msg->z < (1 - arm_th)) { _msg_actuator_armed.armed = true; + _msg_vehicle_control_mode.flag_armed = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; } } /* fill vehicle status */ - //XXX hardcoded msg_vehicle_status.timestamp = px4::get_time_micros(); - msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; - msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; - msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; msg_vehicle_status.is_rotary_wing = true; - _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); _vehicle_status_pub.publish(msg_vehicle_status); @@ -107,6 +101,57 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) { + // XXX this is a minimal implementation. If more advanced functionalities are + // needed consider a full port of the commander + + switch (msg->mode_switch) { + case px4::manual_control_setpoint::SWITCH_POS_NONE: + ROS_WARN("Joystick button mapping error, main mode not set"); + break; + + case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = false; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = false; + msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; + + break; + + case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST + if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_POSCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = true; + msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + msg_vehicle_control_mode.flag_control_altitude_enabled = true; + msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; + msg_vehicle_control_mode.flag_control_position_enabled = false; + msg_vehicle_control_mode.flag_control_velocity_enabled = false; + } + break; + } + +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index bd4092b3a..58b7257b7 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -40,6 +40,8 @@ #include "ros/ros.h" #include <px4/manual_control_setpoint.h> +#include <px4/vehicle_control_mode.h> +#include <px4/vehicle_status.h> #include <px4/parameter_update.h> #include <px4/actuator_armed.h> @@ -56,6 +58,13 @@ protected: */ void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + /** + * Set control mode flags based on stick positions (equiv to code in px4 commander) + */ + void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; ros::Publisher _vehicle_control_mode_pub; @@ -65,5 +74,6 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; + px4::vehicle_control_mode _msg_vehicle_control_mode; }; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 688df50e0..72f6e252f 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -40,13 +40,14 @@ #include "manual_input.h" -#include <px4/manual_control_setpoint.h> #include <platforms/px4_middleware.h> ManualInput::ManualInput() : _n(), _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), - _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)) + _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)), + _buttons_state(), + _msg_mc_sp() { double dz_default = 0.2; /* Load parameters, default values work for Microsoft XBox Controller */ @@ -63,40 +64,46 @@ ManualInput::ManualInput() : _n.param("map_z", _param_axes_map[2], 2); _n.param("scale_z", _param_axes_scale[2], -0.5); _n.param("off_z", _param_axes_offset[2], -1.0); - _n.param("dz_z", _param_axes_dz[2], dz_default); + _n.param("dz_z", _param_axes_dz[2], 0.0); _n.param("map_r", _param_axes_map[3], 0); _n.param("scale_r", _param_axes_scale[3], -1.0); _n.param("off_r", _param_axes_offset[3], 0.0); _n.param("dz_r", _param_axes_dz[3], dz_default); + _n.param("map_manual", _param_buttons_map[0], 0); + _n.param("map_altctl", _param_buttons_map[1], 1); + _n.param("map_posctl", _param_buttons_map[2], 2); + _n.param("map_auto_mission", _param_buttons_map[3], 3); + _n.param("map_auto_loiter", _param_buttons_map[4], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 4); + + /* Default to manual */ + _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + } void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) { - px4::manual_control_setpoint msg_out; /* Fill px4 manual control setpoint topic with contents from ros joystick */ /* Map sticks to x, y, z, r */ - MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x); - MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); - MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); - MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); - //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], _msg_mc_sp.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r); /* Map buttons to switches */ - //XXX todo - /* for now just publish switches in position for manual flight */ - msg_out.mode_switch = 0; - msg_out.return_switch = 0; - msg_out.posctl_switch = 0; - msg_out.loiter_switch = 0; - msg_out.acro_switch = 0; - msg_out.offboard_switch = 0; - - msg_out.timestamp = px4::get_time_micros(); - - _man_ctrl_sp_pub.publish(msg_out); + MapButtons(msg, _msg_mc_sp); + + _msg_mc_sp.timestamp = px4::get_time_micros(); + + _man_ctrl_sp_pub.publish(_msg_mc_sp); } void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, @@ -106,9 +113,42 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { + out = 0.0f; } } +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { + msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; + + if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + + } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + } + +} + int main(int argc, char **argv) { ros::init(argc, argv, "manual_input"); diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index 93e0abe64..bf704f675 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -39,6 +39,7 @@ */ #include "ros/ros.h" +#include <px4/manual_control_setpoint.h> #include <sensor_msgs/Joy.h> class ManualInput @@ -55,20 +56,38 @@ protected: void JoyCallback(const sensor_msgs::JoyConstPtr &msg); /** - * Helper function to map and scale joystick input + * Helper function to map and scale joystick axis */ void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, float &out); + /** + * Helper function to map joystick buttons + */ + void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp); ros::NodeHandle _n; ros::Subscriber _joy_sub; ros::Publisher _man_ctrl_sp_pub; /* Parameters */ + enum MAIN_STATE { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, + MAIN_STATE_MAX + }; + + int _param_buttons_map[MAIN_STATE_MAX]; /**< joystick button mapping, order according to MAIN_STATE */ + bool _buttons_state[MAIN_STATE_MAX]; /**< joystick button state of last iteration, + order according to MAIN_STATE */ + int _param_axes_map[4]; double _param_axes_scale[4]; double _param_axes_offset[4]; double _param_axes_dz[4]; - + px4::manual_control_setpoint _msg_mc_sp; }; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 54f5fa78b..0749c8e92 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -223,19 +223,15 @@ void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_contro _n.getParamCached("mixer", mixer_name); if (mixer_name == "x") { _rotors = _config_index[0]; - ROS_WARN("using x"); } else if (mixer_name == "+") { _rotors = _config_index[1]; } else if (mixer_name == "e") { _rotors = _config_index[2]; } else if (mixer_name == "w") { _rotors = _config_index[3]; - ROS_WARN("using w"); } else if (mixer_name == "i") { _rotors = _config_index[4]; - ROS_WARN("using i"); } - ROS_WARN("mixer_name %s", mixer_name.c_str()); // mix mix(); diff --git a/src/platforms/ros/nodes/position_estimator/position_estimator.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp new file mode 100644 index 000000000..ed3a4efa5 --- /dev/null +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file position_estimator.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Roman Bapst <romanbapst@yahoo.de> +*/ + +#include "position_estimator.h" + +#include <px4/vehicle_local_position.h> +#include <mathlib/mathlib.h> +#include <platforms/px4_defines.h> +#include <platforms/px4_middleware.h> +#include <vector> +#include <string> +#include <gazebo_msgs/ModelStates.h> + +PositionEstimator::PositionEstimator() : + _n(), + _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)), + _vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)), + _startup_time(1) +{ +} + +void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + //XXX: use a proper sensor topic + + px4::vehicle_local_position msg_v_l_pos; + + /* Fill px4 position topic with contents from modelstates topic */ + int index = 0; + //XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when + //gazebo rearranges indexes. + for(std::vector<std::string>::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) { + if (*it == "iris" || *it == "ardrone") { + index = it - msg->name.begin(); + break; + } + } + msg_v_l_pos.xy_valid = true; + msg_v_l_pos.z_valid = true; + msg_v_l_pos.v_xy_valid = true; + msg_v_l_pos.v_z_valid = true; + + msg_v_l_pos.x = msg->pose[index].position.x; + msg_v_l_pos.y = -msg->pose[index].position.y; + msg_v_l_pos.z = -msg->pose[index].position.z; + msg_v_l_pos.vx = msg->twist[index].linear.x; + msg_v_l_pos.vy = -msg->twist[index].linear.y; + msg_v_l_pos.vz = -msg->twist[index].linear.z; + + msg_v_l_pos.xy_global = true; + msg_v_l_pos.z_global = true; + msg_v_l_pos.ref_timestamp = _startup_time; + msg_v_l_pos.ref_lat = 47.378301; + msg_v_l_pos.ref_lon = 8.538777; + msg_v_l_pos.ref_alt = 1200.0f; + + + msg_v_l_pos.timestamp = px4::get_time_micros(); + _vehicle_position_pub.publish(msg_v_l_pos); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "position_estimator"); + PositionEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/ros/nodes/position_estimator/position_estimator.h index ec557e8aa..da1fc2c5a 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/ros/nodes/position_estimator/position_estimator.h @@ -32,13 +32,31 @@ ****************************************************************************/ /** - * @file px4_nodehandle.cpp + * @file position_estimator.h + * Dummy position estimator that forwards position from gazebo to px4 topic * - * PX4 Middleware Wrapper Nodehandle - */ -#include <px4.h> + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ -namespace px4 +#include "ros/ros.h" +#include <gazebo_msgs/ModelStates.h> +#include <sensor_msgs/Imu.h> + +class PositionEstimator { +public: + PositionEstimator(); + + ~PositionEstimator() {} + +protected: + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Publisher _vehicle_position_pub; + + uint64_t _startup_time; + -} +}; diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 4a97d328c..f54342f06 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -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> @@ -58,6 +58,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" __EXPORT int config_main(int argc, char *argv[]); @@ -70,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 @@ -132,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) { @@ -191,8 +192,12 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); close(fd); } @@ -205,41 +210,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) { @@ -264,8 +269,12 @@ do_mag(int argc, char *argv[]) int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } @@ -278,41 +287,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) { @@ -337,8 +346,12 @@ do_accel(int argc, char *argv[]) int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + + param_get(param_find("SENS_ACC_ID"), &(calibration_id)); - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 20967b541..32682f890 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -64,6 +64,10 @@ #include "drivers/drv_pwm_output.h" #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_0.h> +#include <uORB/topics/actuator_controls_1.h> +#include <uORB/topics/actuator_controls_2.h> +#include <uORB/topics/actuator_controls_3.h> static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e..4e2710572 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 ceaea35b6..50547a562 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -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) { diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 86e4ff545..3e1f76714 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; + int32_t devid, calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -96,8 +97,18 @@ int preflight_check_main(int argc, char *argv[]) system_ok = false; goto system_eval; } + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); - + if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); @@ -109,8 +120,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, ACCELIOCSELFTEST, 0); - + if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); @@ -145,8 +166,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(GYRO_DEVICE_PATH, 0); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, GYROIOCSELFTEST, 0); - + if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); @@ -172,10 +203,10 @@ int preflight_check_main(int argc, char *argv[]) system_ok &= rc_ok; - + system_eval: - + if (system_ok) { /* all good, exit silently */ exit(0); diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 2ead3e632..087eb52e3 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"); } |