diff options
181 files changed, 16368 insertions, 2445 deletions
diff --git a/.gitignore b/.gitignore index 0372b60c8..3fdc1bf2a 100644 --- a/.gitignore +++ b/.gitignore @@ -24,4 +24,7 @@ Firmware.sublime-workspace Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ -NuttX
\ No newline at end of file +/NuttX +/Documentation/doxy.log +/Documentation/html/ +/Documentation/doxygen*objdb*tmp
\ No newline at end of file diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py new file mode 100644 index 000000000..b0864a229 --- /dev/null +++ b/Debug/Nuttx.py @@ -0,0 +1,298 @@ +# GDB/Python functions for dealing with NuttX + +import gdb, gdb.types + +class NX_task(object): + """Reference to a NuttX task and methods for introspecting it""" + + def __init__(self, tcb_ptr): + self._tcb = tcb_ptr.dereference() + self._group = self._tcb['group'].dereference() + self.pid = tcb_ptr['pid'] + + @classmethod + def for_tcb(cls, tcb): + """return a task with the given TCB pointer""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + pidhash_entry = pidhash_value[i] + if pidhash_entry['tcb'] == tcb: + return cls(pidhash_entry['tcb']) + return None + + @classmethod + def for_pid(cls, pid): + """return a task for the given PID""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + pidhash_entry = pidhash_value[i] + if pidhash_entry['pid'] == pid: + return cls(pidhash_entry['tcb']) + return None + + @staticmethod + def pids(): + """return a list of all PIDs""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + result = [] + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + entry = pidhash_value[i] + pid = int(entry['pid']) + if pid is not -1: + result.append(pid) + return result + + @staticmethod + def tasks(): + """return a list of all tasks""" + tasks = [] + for pid in NX_task.pids(): + tasks.append(NX_task.for_pid(pid)) + return tasks + + def _state_is(self, state): + """tests the current state of the task against the passed-in state name""" + statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) + if self._tcb['task_state'] == statenames[state]: + return True + return False + + @property + def stack_used(self): + """calculate the stack used by the thread""" + if 'stack_used' not in self.__dict__: + stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer()) + if stack_base == 0: + self.__dict__['stack_used'] = 0 + else: + stack_limit = self._tcb['adj_stack_size'] + for offset in range(0, stack_limit): + if stack_base[offset] != 0xff: + break + self.__dict__['stack_used'] = stack_limit - offset + return self.__dict__['stack_used'] + + @property + def name(self): + """return the task's name""" + return self._tcb['name'].string() + + @property + def state(self): + """return the name of the task's current state""" + statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) + for name,value in statenames.iteritems(): + if value == self._tcb['task_state']: + return name + return 'UNKNOWN' + + @property + def waiting_for(self): + """return a description of what the task is waiting for, if it is waiting""" + if self._state_is('TSTATE_WAIT_SEM'): + waitsem = self._tcb['waitsem'].dereference() + waitsem_holder = waitsem['holder'] + holder = NX_task.for_tcb(waitsem_holder['htcb']) + if holder is not None: + return '{}({})'.format(waitsem.address, holder.name) + else: + return '{}(<bad holder>)'.format(waitsem.address) + if self._state_is('TSTATE_WAIT_SIG'): + return 'signal' + return None + + @property + def is_waiting(self): + """tests whether the task is waiting for something""" + if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'): + return True + + @property + def is_runnable(self): + """tests whether the task is runnable""" + if (self._state_is('TSTATE_TASK_PENDING') or + self._state_is('TSTATE_TASK_READYTORUN') or + self._state_is('TSTATE_TASK_RUNNING')): + return True + return False + + @property + def file_descriptors(self): + """return a dictionary of file descriptors and inode pointers""" + filelist = self._group['tg_filelist'] + filearray = filelist['fl_files'] + result = dict() + for i in range(filearray.type.range()[0],filearray.type.range()[1]): + inode = long(filearray[i]['f_inode']) + if inode != 0: + result[i] = inode + return result + + @property + def registers(self): + if 'registers' not in self.__dict__: + registers = dict() + if self._state_is('TSTATE_TASK_RUNNING'): + # XXX need to fix this to handle interrupt context + registers['R0'] = long(gdb.parse_and_eval('$r0')) + registers['R1'] = long(gdb.parse_and_eval('$r1')) + registers['R2'] = long(gdb.parse_and_eval('$r2')) + registers['R3'] = long(gdb.parse_and_eval('$r3')) + registers['R4'] = long(gdb.parse_and_eval('$r4')) + registers['R5'] = long(gdb.parse_and_eval('$r5')) + registers['R6'] = long(gdb.parse_and_eval('$r6')) + registers['R7'] = long(gdb.parse_and_eval('$r7')) + registers['R8'] = long(gdb.parse_and_eval('$r8')) + registers['R9'] = long(gdb.parse_and_eval('$r9')) + registers['R10'] = long(gdb.parse_and_eval('$r10')) + registers['R11'] = long(gdb.parse_and_eval('$r11')) + registers['R12'] = long(gdb.parse_and_eval('$r12')) + registers['R13'] = long(gdb.parse_and_eval('$r13')) + registers['SP'] = long(gdb.parse_and_eval('$sp')) + registers['R14'] = long(gdb.parse_and_eval('$r14')) + registers['LR'] = long(gdb.parse_and_eval('$lr')) + registers['R15'] = long(gdb.parse_and_eval('$r15')) + registers['PC'] = long(gdb.parse_and_eval('$pc')) + registers['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + # this would only be valid if we were in an interrupt + registers['EXC_RETURN'] = 0 + # we should be able to get this... + registers['PRIMASK'] = 0 + else: + context = self._tcb['xcp'] + regs = context['regs'] + registers['R0'] = long(regs[27]) + registers['R1'] = long(regs[28]) + registers['R2'] = long(regs[29]) + registers['R3'] = long(regs[30]) + registers['R4'] = long(regs[2]) + registers['R5'] = long(regs[3]) + registers['R6'] = long(regs[4]) + registers['R7'] = long(regs[5]) + registers['R8'] = long(regs[6]) + registers['R9'] = long(regs[7]) + registers['R10'] = long(regs[8]) + registers['R11'] = long(regs[9]) + registers['R12'] = long(regs[31]) + registers['R13'] = long(regs[0]) + registers['SP'] = long(regs[0]) + registers['R14'] = long(regs[32]) + registers['LR'] = long(regs[32]) + registers['R15'] = long(regs[33]) + registers['PC'] = long(regs[33]) + registers['XPSR'] = long(regs[34]) + registers['EXC_RETURN'] = long(regs[10]) + registers['PRIMASK'] = long(regs[1]) + self.__dict__['registers'] = registers + return self.__dict__['registers'] + + def __repr__(self): + return "<NX_task {}>".format(self.pid) + + def __str__(self): + return "{}:{}".format(self.pid, self.name) + + def __format__(self, format_spec): + return format_spec.format( + pid = self.pid, + name = self.name, + state = self.state, + waiting_for = self.waiting_for, + stack_used = self.stack_used, + stack_limit = self._tcb['adj_stack_size'], + file_descriptors = self.file_descriptors, + registers = self.registers + ) + +class NX_show_task (gdb.Command): + """(NuttX) prints information about a task""" + + def __init__(self): + super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) + + def invoke(self, arg, from_tty): + t = NX_task.for_pid(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' + if t.is_waiting: + my_fmt += ' waiting for {waiting_for}\n' + my_fmt += ' open files: {file_descriptors}\n' + my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n' + my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n' + my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' + my_fmt += ' R12 {registers[PC]:#010x}\n' + my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' + print format(t, my_fmt) + +class NX_show_tasks (gdb.Command): + """(NuttX) prints a list of tasks""" + + def __init__(self): + super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER) + + def invoke(self, args, from_tty): + tasks = NX_task.tasks() + for t in tasks: + print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + +NX_show_task() +NX_show_tasks() + +class NX_show_heap (gdb.Command): + """(NuttX) prints the heap""" + + def __init__(self): + super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) + if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8: + self._allocflag = 0x80000000 + self._allocnodesize = 8 + else: + self._allocflag = 0x8000 + self._allocnodesize = 4 + + def _node_allocated(self, allocnode): + if allocnode['preceding'] & self._allocflag: + return True + return False + + def _node_size(self, allocnode): + return allocnode['size'] & ~self._allocflag + + def _print_allocations(self, region_start, region_end): + if region_start >= region_end: + print 'heap region {} corrupt'.format(hex(region_start)) + return + nodecount = region_end - region_start + print 'heap {} - {}'.format(region_start, region_end) + cursor = 1 + while cursor < nodecount: + allocnode = region_start[cursor] + if self._node_allocated(allocnode): + state = '' + else: + state = '(free)' + print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state) + cursor += self._node_size(allocnode) / self._allocnodesize + + def invoke(self, args, from_tty): + heap = gdb.lookup_global_symbol('g_mmheap').value() + nregions = heap['mm_nregions'] + region_starts = heap['mm_heapstart'] + region_ends = heap['mm_heapend'] + print "{} heap(s)".format(nregions) + # walk the heaps + for i in range(0, nregions): + self._print_allocations(region_starts[i], region_ends[i]) + +NX_show_heap() + + + + diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 4802ef818..241702811 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME = NO # disable (NO) the todo list. This list is created by putting \todo # commands in the documentation. -GENERATE_TODOLIST = YES +GENERATE_TODOLIST = NO # The GENERATE_TESTLIST tag can be used to enable (YES) or # disable (NO) the test list. This list is created by putting \test # commands in the documentation. -GENERATE_TESTLIST = YES +GENERATE_TESTLIST = NO # The GENERATE_BUGLIST tag can be used to enable (YES) or # disable (NO) the bug list. This list is created by putting \bug # commands in the documentation. -GENERATE_BUGLIST = YES +GENERATE_BUGLIST = NO # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or # disable (NO) the deprecated list. This list is created by putting @@ -569,7 +569,7 @@ WARN_LOGFILE = doxy.log # directories like "/usr/src/myproject". Separate the files or directories # with spaces. -INPUT = ../apps +INPUT = ../src # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is @@ -599,9 +599,8 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../dist/ \ - ../docs/html/ \ - html +EXCLUDE = ../src/modules/mathlib/CMSIS \ + ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or # directories that are symbolic links (a Unix filesystem feature) are excluded diff --git a/Documentation/commander_app.odg b/Documentation/commander_app.odg Binary files differdeleted file mode 100644 index 17459f7cf..000000000 --- a/Documentation/commander_app.odg +++ /dev/null diff --git a/Documentation/commander_app.png b/Documentation/commander_app.png Binary files differdeleted file mode 100644 index 6503817da..000000000 --- a/Documentation/commander_app.png +++ /dev/null diff --git a/Documentation/control_flow.graffle b/Documentation/control_flow.graffle index 253523110..6188c4430 100644 --- a/Documentation/control_flow.graffle +++ b/Documentation/control_flow.graffle @@ -5,7 +5,7 @@ <key>ApplicationVersion</key> <array> <string>com.omnigroup.OmniGraffle</string> - <string>139.16.0.171715</string> + <string>139.18.0.187838</string> </array> <key>CreationDate</key> <string>2013-02-22 17:51:02 +0000</string> @@ -26,9 +26,9 @@ <key>MasterSheets</key> <array/> <key>ModificationDate</key> - <string>2013-04-20 15:38:47 +0000</string> + <string>2013-08-25 05:58:40 +0000</string> <key>Modifier</key> - <string>Lorenz Meier</string> + <string>Lorenz</string> <key>NotesVisible</key> <string>NO</string> <key>OriginVisible</key> @@ -55,7 +55,7 @@ <key>NSPaperName</key> <array> <string>string</string> - <string>iso-a3</string> + <string>BADB3137-012D-43BB-AFDE-2C0069A874AD</string> </array> <key>NSPaperSize</key> <array> @@ -123,7 +123,7 @@ <array> <dict> <key>Bounds</key> - <string>{{320.41170773935767, 646.55758982070449}, {100, 24}}</string> + <string>{{320.41170773792788, 646.55758667263535}, {100, 24}}</string> <key>Class</key> <string>ShapedGraphic</string> <key>FitText</key> @@ -171,7 +171,7 @@ <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -193,8 +193,8 @@ <integer>45</integer> <key>Points</key> <array> - <string>{283.47949897905681, 658.66217570036315}</string> - <string>{459.99996984645378, 658.44980851233595}</string> + <string>{283.47949897630235, 658.66217426345122}</string> + <string>{459.99996984638915, 658.4498036008282}</string> </array> <key>Style</key> <dict> @@ -243,7 +243,7 @@ <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -289,7 +289,7 @@ <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -340,7 +340,7 @@ <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural @@ -389,7 +389,7 @@ <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -437,7 +437,7 @@ <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;\red68\green147\blue53;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -461,8 +461,8 @@ <integer>38</integer> <key>Points</key> <array> - <string>{584.52245687751122, 125.49999965650382}</string> - <string>{584.56069426576869, 157.00000090441219}</string> + <string>{584.50076858662646, 125.50000000000021}</string> + <string>{584.50207726115275, 157.00000000001324}</string> </array> <key>Style</key> <dict> @@ -511,7 +511,7 @@ <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -532,8 +532,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>34</integer> <key>Points</key> <array> - <string>{210.47275259888471, 495.87499981747118}</string> - <string>{210.43193555768113, 543.06250166479344}</string> + <string>{210.49312053369275, 495.87499999999562}</string> + <string>{210.48281498396668, 543.06249999958038}</string> </array> <key>Style</key> <dict> @@ -582,7 +582,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -602,8 +602,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>32</integer> <key>Points</key> <array> - <string>{210.46968123779743, 580.06249992316259}</string> - <string>{210.44442316539605, 627.25000030102035}</string> + <string>{210.47945786724097, 580.0625}</string> + <string>{210.47913680605853, 627.24999999999989}</string> </array> <key>Style</key> <dict> @@ -637,8 +637,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>30</integer> <key>Points</key> <array> - <string>{210.48620562018775, 690.25}</string> - <string>{210.49612530146715, 737.4375}</string> + <string>{210.48620103078346, 690.25}</string> + <string>{210.49611383706232, 737.4375}</string> </array> <key>Style</key> <dict> @@ -694,7 +694,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -731,7 +731,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -789,7 +789,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -801,7 +801,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> </dict> <dict> <key>Bounds</key> - <string>{{152.49994329565658, 397.00120984204113}, {116, 24}}</string> + <string>{{152.49998995675264, 397.00120984204113}, {116, 24}}</string> <key>Class</key> <string>ShapedGraphic</string> <key>FitText</key> @@ -849,7 +849,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -877,8 +877,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <real>-1</real> <key>Points</key> <array> - <string>{584.49994992834695, 220}</string> - <string>{584.4997453697481, 348.6875000000008}</string> + <string>{584.49999370649277, 220}</string> + <string>{584.49999370649277, 348.6875}</string> </array> <key>Style</key> <dict> @@ -971,7 +971,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -991,8 +991,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>17</integer> <key>Points</key> <array> - <string>{210.49997491180866, 385.6875}</string> - <string>{210.49991091996927, 432.875}</string> + <string>{210.49998995675264, 385.6875}</string> + <string>{210.49998995675264, 432.875}</string> </array> <key>Style</key> <dict> @@ -1029,7 +1029,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Points</key> <array> <string>{283.49999999999511, 270.00001907176534}</string> - <string>{339.50000000000028, 270.00001907176534}</string> + <string>{339.49999999999994, 270.00001907176534}</string> </array> <key>Style</key> <dict> @@ -1098,8 +1098,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>13</integer> <key>Points</key> <array> - <string>{210.49171354592596, 207}</string> - <string>{210.49498558851923, 238.4999999999998}</string> + <string>{210.49209551674531, 207}</string> + <string>{210.49601794208507, 238.5}</string> </array> <key>Style</key> <dict> @@ -1133,8 +1133,8 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <integer>12</integer> <key>Points</key> <array> - <string>{210.49498558851246, 138.5000000000002}</string> - <string>{210.48997117702493, 170.00000000001342}</string> + <string>{210.49601794208485, 138.5}</string> + <string>{210.49203588416967, 170}</string> </array> <key>Style</key> <dict> @@ -1190,7 +1190,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1234,7 +1234,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1278,7 +1278,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1322,7 +1322,7 @@ VEHICLE_LOCAL_POSITION_SETPOINT}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1360,7 +1360,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1395,7 +1395,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1430,7 +1430,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1675,7 +1675,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -1774,7 +1774,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2145,7 +2145,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2536,7 +2536,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -2927,7 +2927,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3361,7 +3361,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3404,7 +3404,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -3791,7 +3791,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4178,7 +4178,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4565,7 +4565,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -4963,7 +4963,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5062,7 +5062,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5421,7 +5421,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -5780,7 +5780,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6175,7 +6175,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -6660,7 +6660,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7019,7 +7019,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7406,7 +7406,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -7793,7 +7793,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8300,7 +8300,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8404,7 +8404,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -8795,7 +8795,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9166,7 +9166,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9537,7 +9537,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -9928,7 +9928,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10367,7 +10367,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10779,7 +10779,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -10883,7 +10883,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11270,7 +11270,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11629,7 +11629,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -11988,7 +11988,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12375,7 +12375,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -12810,7 +12810,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13263,6 +13263,1697 @@ PX4IO or PX4FMU}</string> <key>Graphics</key> <array> <dict> + <key>Bounds</key> + <string>{{281.4621559838219, 891.87422762618871}, {48, 14}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>FitText</key> + <string>YES</string> + <key>Flow</key> + <string>Resize</string> + <key>ID</key> + <integer>1534</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Pad</key> + <integer>0</integer> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\b\fs24 \cf0 QUAD +\b0 H}</string> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>Wrap</key> + <string>NO</string> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{282.10284531231378, 729.4992324185539}, {50.666667938232422, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1536</integer> + <key>Rotation</key> + <real>270</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.13195</string> + <key>g</key> + <string>0.165669</string> + <key>r</key> + <string>1</string> + </dict> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{271.43617794923421, 713.47324050519865}, {72, 72}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1537</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>1535</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{387.95096684220994, 833.99511602139876}, {24, 24}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1539</integer> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 4}</string> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1541</integer> + <key>Points</key> + <array> + <string>{447.56635253821332, 822.0678021003348}</string> + <string>{445.35664384700522, 823.21685061976291}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{359.66328626413321, 875.75030562593793}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1542</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{349.2634215034181, 795.53620935597417}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1543</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{346.5134215034181, 792.78620935597417}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1544</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + </array> + <key>ID</key> + <integer>1540</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1546</integer> + <key>Points</key> + <array> + <string>{352.51941963025371, 870.09214125245364}</string> + <string>{354.7291283214617, 868.94309273302531}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{424.42248590433371, 797.90963772685075}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1547</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{349.32235066504887, 795.12373399681428}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1548</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{346.57235066504887, 792.37373399681428}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1549</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + </array> + <key>ID</key> + <integer>1545</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{349.03928365032027, 795.08342417943641}, {101.82337649086284, 101.82337649086284}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1550</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>a</key> + <string>0.78</string> + <key>b</key> + <string>0.234512</string> + <key>g</key> + <string>0.673128</string> + <key>r</key> + <string>0.148947</string> + </dict> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>1538</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{202.98738428569231, 833.92908910567212}, {24, 24}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1552</integer> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 2}</string> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1554</integer> + <key>Points</key> + <array> + <string>{167.72554658547227, 821.64822718882488}</string> + <string>{169.93525527668044, 822.79727570825287}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{239.62861285955245, 875.33073071442777}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1555</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{164.52847762026795, 795.1166344444639}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1556</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{161.77847762026795, 792.3666344444639}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1557</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1553</integer> + <key>Rotation</key> + <real>225</real> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1559</integer> + <key>Points</key> + <array> + <string>{262.41893329590295, 870.02611253847215}</string> + <string>{260.20922460469507, 868.87706401904404}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{174.51586702182317, 797.84360901286959}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1560</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{164.11600226110755, 795.05770528283301}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1561</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{161.36600226110755, 792.30770528283301}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1562</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1558</integer> + <key>Rotation</key> + <real>225</real> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{164.07569244372991, 795.01739580669891}, {101.82337649086284, 101.82337649086284}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1563</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>a</key> + <string>0.78</string> + <key>b</key> + <string>0.673128</string> + <key>g</key> + <string>0.534129</string> + <key>r</key> + <string>0.217952</string> + </dict> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1551</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{424.02444829866442, 666.20122319568361}, {24, 24}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1565</integer> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 1}</string> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1567</integer> + <key>Points</key> + <array> + <string>{388.76261059844421, 653.92036127883637}</string> + <string>{390.97231928965209, 655.06940979826459}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{460.66567687252439, 707.60286480443915}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1568</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{385.56554163323983, 627.3887685344755}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1569</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{382.81554163323983, 624.6387685344755}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1570</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1566</integer> + <key>Rotation</key> + <real>225</real> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1572</integer> + <key>Points</key> + <array> + <string>{483.455997308875, 702.29824662848364}</string> + <string>{481.24628861766723, 701.14919810905553}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{395.55293103479517, 630.11574310288108}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1573</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{385.15306627407972, 627.32983937284462}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1574</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{382.40306627407972, 624.57983937284462}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1575</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1571</integer> + <key>Rotation</key> + <real>225</real> + <key>VFlip</key> + <string>YES</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{385.11275645670196, 627.28952989671052}, {101.82337649086284, 101.82337649086284}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1576</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>a</key> + <string>0.78</string> + <key>b</key> + <string>0.673128</string> + <key>g</key> + <string>0.534129</string> + <key>r</key> + <string>0.217952</string> + </dict> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>VFlip</key> + <string>YES</string> + </dict> + </array> + <key>ID</key> + <integer>1564</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{170.01738471163631, 670.02445557113811}, {24, 24}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1578</integer> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 3}</string> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1580</integer> + <key>Points</key> + <array> + <string>{229.63277040763995, 658.09714165007426}</string> + <string>{227.42306171643185, 659.24619016950237}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{141.72970413355978, 711.77964517567739}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1581</integer> + <key>Rotation</key> + <real>45</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{131.32983937284456, 631.5655489057134}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1582</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{128.57983937284456, 628.8155489057134}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1583</integer> + <key>Rotation</key> + <real>135</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + </array> + <key>ID</key> + <integer>1579</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>1585</integer> + <key>Points</key> + <array> + <string>{134.58583749968011, 706.12148080219276}</string> + <string>{136.79554619088805, 704.97243228276466}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>0</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{206.48890377375986, 633.93897727658998}, {16, 18.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1586</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>HorizontalTriangle</string> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{131.38876853447516, 631.15307354655363}, {101.5, 101.5}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1587</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{128.63876853447516, 628.40307354655363}, {107, 107}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>HFlip</key> + <string>YES</string> + <key>ID</key> + <integer>1588</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>AdjustableArc</string> + <key>ShapeData</key> + <dict> + <key>endAngle</key> + <real>72</real> + <key>startAngle</key> + <real>281</real> + </dict> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + <key>TextRelativeArea</key> + <string>{{0.10000000000000001, 0.14999999999999999}, {0.80000000000000004, 0.69999999999999996}}</string> + </dict> + </array> + <key>ID</key> + <integer>1584</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{131.1057015197467, 631.11276372917564}, {101.82337649086284, 101.82337649086284}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1589</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>a</key> + <string>0.78</string> + <key>b</key> + <string>0.234512</string> + <key>g</key> + <string>0.673128</string> + <key>r</key> + <string>0.148947</string> + </dict> + </dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>1577</integer> + <key>Rotation</key> + <real>315</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{253.2315878349163, 736.67782196944336}, {8, 134}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1590</integer> + <key>Rotation</key> + <real>225</real> + <key>Shape</key> + <string>Rectangle</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{371.51084102985902, 644.26864825135988}, {8, 134}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1591</integer> + <key>Rotation</key> + <real>240</real> + <key>Shape</key> + <string>Rectangle</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{353.64074716687509, 736.67780398678804}, {8, 134}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1592</integer> + <key>Rotation</key> + <real>315</real> + <key>Shape</key> + <string>Rectangle</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{234.00404905414581, 645.26862880510475}, {8, 134}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1593</integer> + <key>Rotation</key> + <real>300</real> + <key>Shape</key> + <string>Rectangle</string> + </dict> + </array> + <key>ID</key> + <integer>1533</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> <key>Class</key> <string>Group</string> <key>Graphics</key> @@ -13353,7 +15044,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -13744,7 +15435,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14135,7 +15826,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14526,7 +16217,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14928,7 +16619,7 @@ PX4IO or PX4FMU}</string> <key>Pad</key> <integer>0</integer> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -14966,7 +16657,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15337,7 +17028,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -15708,7 +17399,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16115,7 +17806,7 @@ PX4IO or PX4FMU}</string> <key>Text</key> <dict> <key>Text</key> - <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf390 \cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} {\colortbl;\red255\green255\blue255;} \pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc @@ -16581,6 +18272,94 @@ PX4IO or PX4FMU}</string> <key>VPages</key> <integer>1</integer> </dict> + <dict> + <key>ActiveLayerIndex</key> + <integer>0</integer> + <key>AutoAdjust</key> + <true/> + <key>BackgroundGraphic</key> + <dict> + <key>Bounds</key> + <string>{{0, 0}, {806, 1132}}</string> + <key>Class</key> + <string>SolidGraphic</string> + <key>ID</key> + <integer>2</integer> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + </dict> + <key>BaseZoom</key> + <integer>0</integer> + <key>CanvasOrigin</key> + <string>{0, 0}</string> + <key>ColumnAlign</key> + <integer>1</integer> + <key>ColumnSpacing</key> + <real>36</real> + <key>DisplayScale</key> + <string>1 0/72 in = 1.0000 in</string> + <key>GraphicsList</key> + <array/> + <key>GridInfo</key> + <dict/> + <key>HPages</key> + <integer>1</integer> + <key>KeepToScale</key> + <false/> + <key>Layers</key> + <array> + <dict> + <key>Lock</key> + <string>NO</string> + <key>Name</key> + <string>Ebene 1</string> + <key>Print</key> + <string>YES</string> + <key>View</key> + <string>YES</string> + </dict> + </array> + <key>LayoutInfo</key> + <dict> + <key>Animate</key> + <string>NO</string> + <key>circoMinDist</key> + <real>18</real> + <key>circoSeparation</key> + <real>0.0</real> + <key>layoutEngine</key> + <string>dot</string> + <key>neatoSeparation</key> + <real>0.0</real> + <key>twopiSeparation</key> + <real>0.0</real> + </dict> + <key>Orientation</key> + <integer>2</integer> + <key>PrintOnePage</key> + <false/> + <key>RowAlign</key> + <integer>1</integer> + <key>RowSpacing</key> + <real>36</real> + <key>SheetTitle</key> + <string>Arbeitsfläche 6</string> + <key>UniqueID</key> + <integer>6</integer> + <key>VPages</key> + <integer>1</integer> + </dict> </array> <key>SmartAlignmentGuidesActive</key> <string>YES</string> @@ -16600,7 +18379,7 @@ PX4IO or PX4FMU}</string> </dict> </array> <key>Frame</key> - <string>{{96, 56}, {1043, 822}}</string> + <string>{{170, 4}, {1043, 774}}</string> <key>ListView</key> <true/> <key>OutlineWidth</key> @@ -16614,7 +18393,7 @@ PX4IO or PX4FMU}</string> <key>SidebarWidth</key> <integer>120</integer> <key>VisibleRegion</key> - <string>{{-51, -33}, {908, 683}}</string> + <string>{{-51, 366}, {908, 635}}</string> <key>Zoom</key> <real>1</real> <key>ZoomValues</key> @@ -16644,6 +18423,11 @@ PX4IO or PX4FMU}</string> <real>1</real> <real>1</real> </array> + <array> + <string>Arbeitsfläche 6</string> + <real>1</real> + <real>1</real> + </array> </array> </dict> </dict> diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt Binary files differnew file mode 100644 index 000000000..587a38883 --- /dev/null +++ b/Documentation/dsm_bind.odt diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf Binary files differnew file mode 100644 index 000000000..76155569e --- /dev/null +++ b/Documentation/dsm_bind.pdf diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg Binary files differindex b630ecb40..f9fa7a032 100644 --- a/Documentation/flight_mode_state_machine.odg +++ b/Documentation/flight_mode_state_machine.odg diff --git a/Documentation/mixing_architecture.graffle b/Documentation/mixing_architecture.graffle new file mode 100644 index 000000000..da8027bf7 --- /dev/null +++ b/Documentation/mixing_architecture.graffle @@ -0,0 +1,4398 @@ +<?xml version="1.0" encoding="UTF-8"?> +<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd"> +<plist version="1.0"> +<dict> + <key>ActiveLayerIndex</key> + <integer>0</integer> + <key>ApplicationVersion</key> + <array> + <string>com.omnigroup.OmniGraffle</string> + <string>139.17.0.185490</string> + </array> + <key>AutoAdjust</key> + <true/> + <key>BackgroundGraphic</key> + <dict> + <key>Bounds</key> + <string>{{0, 0}, {1118, 783}}</string> + <key>Class</key> + <string>SolidGraphic</string> + <key>ID</key> + <integer>2</integer> + <key>Style</key> + <dict> + <key>shadow</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + <key>stroke</key> + <dict> + <key>Draws</key> + <string>NO</string> + </dict> + </dict> + </dict> + <key>BaseZoom</key> + <integer>0</integer> + <key>CanvasOrigin</key> + <string>{0, 0}</string> + <key>ColumnAlign</key> + <integer>1</integer> + <key>ColumnSpacing</key> + <real>36</real> + <key>CreationDate</key> + <string>2013-06-04 09:23:13 +0000</string> + <key>Creator</key> + <string>Lorenz Meier</string> + <key>DisplayScale</key> + <string>1 0/72 in = 1.0000 in</string> + <key>GraphDocumentVersion</key> + <integer>8</integer> + <key>GraphicsList</key> + <array> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>508</integer> + </dict> + <key>ID</key> + <integer>635</integer> + <key>Points</key> + <array> + <string>{106.17826841821868, 273.42634001636537}</string> + <string>{213.16101457128596, 357.82365814026997}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>613</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>507</integer> + </dict> + <key>ID</key> + <integer>634</integer> + <key>Points</key> + <array> + <string>{131.96398352136816, 273.42634001634866}</string> + <string>{238.946729674436, 357.82365813972365}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>612</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{482, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>617</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{456.21429061889648, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>618</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{430.42857551574707, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>619</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{404.64286041259766, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>620</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{378.85714530944824, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>621</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{353.07143020629883, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>622</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{327.28571510314941, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>623</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{301.5, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>624</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{482, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>625</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{456.21429061889648, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>626</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{430.42857551574707, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>627</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{404.64286041259766, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>628</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{378.85714530944824, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>629</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{353.07143020629883, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>630</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{327.28571510314941, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>631</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{301.5, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>632</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{291.5, 235.24999816050627}, {208, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>633</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 Actuator Control Group 1}</string> + </dict> + </dict> + </array> + <key>ID</key> + <integer>616</integer> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>587</integer> + </dict> + <key>ID</key> + <integer>592</integer> + <key>Points</key> + <array> + <string>{605.82627753848249, 74.5}</string> + <string>{716.81658859616107, 159.07205874840687}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>581</integer> + </dict> + <key>ID</key> + <integer>591</integer> + <key>Points</key> + <array> + <string>{576.14671565757703, 75.907219770052052}</string> + <string>{565.99614475502062, 157.66483897835485}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>590</integer> + <key>Points</key> + <array> + <string>{565.49194626385997, 201.66102838314328}</string> + <string>{565.21582473837202, 338.8005880984627}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>573</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>ID</key> + <integer>589</integer> + <key>Points</key> + <array> + <string>{720.20623661457341, 201.6610283823652}</string> + <string>{719.9301058948314, 338.80058788427203}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>579</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{743, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>588</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{717.21429061889648, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>587</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{691.42857551574707, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>586</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{665.64286041259766, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>585</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{639.85714530944824, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>584</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{614.07143020629883, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>583</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{588.28571510314941, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>582</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{562.5, 158.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>581</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{743, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>580</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{717.21429061889648, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>579</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{691.42857551574707, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>578</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{665.64286041259766, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>577</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{639.85714530944824, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>576</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{614.07143020629883, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>575</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{588.28571510314941, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>574</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{562.5, 194.16102937420345}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>573</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{552.5, 162.16102937420345}, {208, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>572</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 RC Scaling & Mapping}</string> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>509</integer> + </dict> + <key>ID</key> + <integer>569</integer> + <key>Points</key> + <array> + <string>{80.392553315069293, 273.42634001641937}</string> + <string>{187.37529946813521, 357.82365814202342}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>614</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>506</integer> + </dict> + <key>ID</key> + <integer>596</integer> + <key>Points</key> + <array> + <string>{378.46463924118007, 273.4271429546975}</string> + <string>{271.51750416091619, 357.82285520170717}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>629</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{254.5, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>599</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{228.71429061889648, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>600</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{202.92857551574707, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>601</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{177.14286041259766, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>602</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{151.35714530944824, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>603</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{125.57143020629883, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>604</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{99.785715103149414, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>605</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{74, 231.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>606</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{254.5, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>607</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{228.71429061889648, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>608</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{202.92857551574707, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>609</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{177.14286041259766, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>610</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{151.35714530944824, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>611</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{125.57143020629883, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>612</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{99.785715103149414, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>613</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{74, 267.24999816050627}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>614</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{64, 235.24999816050627}, {208, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>615</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 Actuator Control Group 0}</string> + </dict> + </dict> + </array> + <key>ID</key> + <integer>598</integer> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>474</integer> + </dict> + <key>ID</key> + <integer>411</integer> + <key>Points</key> + <array> + <string>{322.59687445702775, 444.84226761008853}</string> + <string>{379.20669915150745, 526.90773238985753}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>510</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>477</integer> + </dict> + <key>ID</key> + <integer>412</integer> + <key>Points</key> + <array> + <string>{294.62113860994009, 444.9967148570675}</string> + <string>{304.03957452010235, 526.7532851429479}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>511</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>490</integer> + </dict> + <key>ID</key> + <integer>413</integer> + <key>Points</key> + <array> + <string>{290.76782961936522, 443.67073351699491}</string> + <string>{183.53574398902853, 528.07926647675936}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>511</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>491</integer> + </dict> + <key>ID</key> + <integer>414</integer> + <key>Points</key> + <array> + <string>{264.9821145162158, 443.67073351699491}</string> + <string>{157.75002888587912, 528.07926647675936}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>512</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>492</integer> + </dict> + <key>ID</key> + <integer>415</integer> + <key>Points</key> + <array> + <string>{239.19639941306636, 443.67073351699491}</string> + <string>{131.9643137827297, 528.07926647675936}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>513</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>493</integer> + </dict> + <key>ID</key> + <integer>416</integer> + <key>Points</key> + <array> + <string>{213.41068430991695, 443.67073351699491}</string> + <string>{106.1785986795803, 528.07926647675936}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>514</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>440</integer> + </dict> + <key>ID</key> + <integer>417</integer> + <key>Points</key> + <array> + <string>{304.49194626386003, 570.74999900893988}</string> + <string>{304.21582473837134, 707.88955872425925}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>485</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>429</integer> + </dict> + <key>ID</key> + <integer>418</integer> + <key>Points</key> + <array> + <string>{381.84909342488743, 570.74999900816329}</string> + <string>{381.57303538097813, 707.88955851048831}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>482</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{379.35432048604451, 719.00347978513389}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>421</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{396.74606011338682, 726.18083608931011}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>422</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{391.6252245834844, 724.37750550833903}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>423</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{386.29980802056684, 722.50213058402392}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>424</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{375.67643661590967, 718.76105153253593}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>425</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{370.35101205342835, 716.88567379113056}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>426</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{365.23018452308935, 715.08234602724974}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>427</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{345.93778549185652, 717.4003115566619}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>428</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>420</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{371.98798359082144, 708.38955751823028}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>429</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>419</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{301.99718791759841, 719.00348000010274}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>432</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{319.38892754494066, 726.18083630427896}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>433</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{314.26809201503823, 724.37750572330788}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>434</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{308.94267545212068, 722.50213079899277}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>435</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{298.3193040474635, 718.76105174750478}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>436</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{292.99387948498219, 716.88567400609941}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>437</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{287.87305195464319, 715.08234624221859}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>438</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{268.58065292341041, 717.40031177163075}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>439</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>431</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{294.63085102237534, 708.38955773319913}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>440</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>430</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{151.85433513439526, 676.40348606783687}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>443</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{169.24607476173759, 683.58084237201308}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>444</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{164.12523923183511, 681.777511791042}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>445</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{158.79982266891756, 679.90213686672689}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>446</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{148.17645126426038, 676.16105781523891}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>447</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{142.85102670177906, 674.28568007383353}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>448</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{137.73019917144009, 672.48235230995272}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>449</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{118.43780014020726, 674.80031783936488}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>450</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>442</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{144.48799823917219, 665.78956380093325}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>451</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>441</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{126.06861430919994, 649.30352977035523}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>454</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{143.46035393654228, 656.48088607453144}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>455</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{138.3395184066398, 654.67755549356036}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>456</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{133.01410184372224, 652.80218056924525}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>457</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{122.39073043906507, 649.06110151775727}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>458</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{117.06530587658375, 647.18572377635189}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>459</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{111.94447834624478, 645.38239601247108}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>460</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{92.652079315011946, 647.70036154188324}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>461</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>453</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{118.70227741397687, 638.68960750345161}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>462</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>452</integer> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>494</integer> + </dict> + <key>ID</key> + <integer>463</integer> + <key>Points</key> + <array> + <string>{187.62496920676753, 443.67073351699486}</string> + <string>{80.392883576430918, 528.07926647675936}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>515</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>451</integer> + </dict> + <key>ID</key> + <integer>464</integer> + <key>Points</key> + <array> + <string>{154.34620841108131, 570.74999811167606}</string> + <string>{154.08771587848975, 665.2895656892573}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>499</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>462</integer> + </dict> + <key>ID</key> + <integer>465</integer> + <key>Points</key> + <array> + <string>{128.55726775049197, 570.74999689176411}</string> + <string>{128.31848992771231, 638.1896106116875}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>500</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>538</integer> + </dict> + <key>ID</key> + <integer>466</integer> + <key>Points</key> + <array> + <string>{102.76581371331335, 570.74999379087535}</string> + <string>{102.56212456317996, 611.68961371257615}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>501</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>549</integer> + </dict> + <key>ID</key> + <integer>467</integer> + <key>Points</key> + <array> + <string>{76.966539516560758, 570.74998251647355}</string> + <string>{76.84574963416425, 585.18962498697681}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>502</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>550</integer> + </dict> + <key>ID</key> + <integer>468</integer> + <key>Points</key> + <array> + <string>{180.14284120598458, 570.75}</string> + <string>{180.14284120598458, 685.34997558593727}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>498</integer> + </dict> + </dict> + <dict> + <key>Class</key> + <string>LineGraphic</string> + <key>Head</key> + <dict> + <key>ID</key> + <integer>527</integer> + </dict> + <key>ID</key> + <integer>469</integer> + <key>Points</key> + <array> + <string>{205.92052173345891, 570.74999897014993}</string> + <string>{205.64439858308342, 707.8895785636937}</string> + </array> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>HeadArrow</key> + <string>FilledArrow</string> + <key>Legacy</key> + <true/> + <key>TailArrow</key> + <string>0</string> + </dict> + </dict> + <key>Tail</key> + <dict> + <key>ID</key> + <integer>497</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{482, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>470</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{456.21429061889648, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>471</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{430.42857551574707, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>472</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{404.64286041259766, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>473</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{378.85714530944824, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>474</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{353.07143020629883, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>475</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{327.28571510314941, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>476</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{301.5, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>477</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{482, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>478</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{456.21429061889648, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>479</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{430.42857551574707, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>480</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{404.64286041259766, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>481</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{378.85714530944824, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>482</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{353.07143020629883, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>483</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{327.28571510314941, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>484</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{301.5, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>485</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{291.5, 531.25}, {208, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>486</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 Actuator Output Group 1 (e.g. FMU)}</string> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{254.5, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>487</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{228.71429061889648, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>488</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{202.92857551574707, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>489</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{177.14286041259766, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>490</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{151.35714530944824, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>491</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{125.57143020629883, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>492</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{99.785715103149414, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>493</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{74, 527.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>494</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{254.5, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>495</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{228.71429061889648, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>496</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{202.92857551574707, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>497</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{177.14286041259766, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>498</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{151.35714530944824, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>499</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{125.57143020629883, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>500</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{99.785715103149414, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>501</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{74, 563.25}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>502</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{64, 531.25}, {208, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>503</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 Actuator Output Group 0 (e.g. IO)}</string> + </dict> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{265.12499809265137, 357}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>506</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{239.33928298950195, 357}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>507</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{213.55356788635254, 357}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>508</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{187.76785278320312, 357}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>509</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{316.9464282989502, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>510</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{291.16071319580078, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>511</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{265.37499809265137, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>512</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{239.58928298950195, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>513</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{213.80356788635254, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>514</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{188.01785278320312, 437.5}, {6, 7}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>515</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>1</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>505</integer> + </dict> + <dict> + <key>Bounds</key> + <string>{{177.14285278320312, 359.75}, {208, 82}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>516</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>9</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 Vehicle Mixer}</string> + </dict> + </dict> + </array> + <key>ID</key> + <integer>504</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{203.42576152599673, 719.00349980074725}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>519</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{220.81750115333907, 726.18085610492346}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>520</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{215.69666562343659, 724.37752552395239}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>521</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{210.37124906051903, 722.50215059963728}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>522</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{199.74787765586186, 718.76107154814929}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>523</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{194.42245309338054, 716.88569380674392}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>524</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{189.30162556304157, 715.0823660428631}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>525</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{170.00922653180874, 717.40033157227526}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>526</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>518</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{196.05942463077366, 708.38957753384364}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>527</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>517</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{100.28290111339905, 622.80352977035523}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>530</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{117.67464074074138, 629.98088607453144}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>531</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{112.5538052108389, 628.17755549356036}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>532</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{107.22838864792135, 626.30218056924525}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>533</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{96.605017243264172, 622.56110151775727}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>534</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{91.279592680782855, 620.68572377635189}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>535</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{86.158765150443884, 618.88239601247108}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>536</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{66.866366119211051, 621.20036154188324}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>537</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>529</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{92.916564218175978, 612.18960750345161}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>538</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>528</integer> + </dict> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Class</key> + <string>Group</string> + <key>Graphics</key> + <array> + <dict> + <key>Bounds</key> + <string>{{74.49718852803673, 596.30352977035523}, {5.0056232242123722, 4.9183513058348947}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>541</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{91.888928155379034, 603.48088607453144}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>542</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{86.768092625476555, 601.67755549356036}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>543</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{81.442676062559002, 599.80218056924525}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>544</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{70.819304657901824, 596.06110151775727}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>545</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{65.493880095420508, 594.18572377635189}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>546</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{60.373052565081537, 592.38239601247108}, {1.9456694882012362, 1.7352345929064885}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>547</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Circle</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{41.080653533848704, 594.70036154188324}, {71.838699340820256, 8.1246847180976189}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>548</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + </array> + <key>ID</key> + <integer>540</integer> + <key>Rotation</key> + <real>19.399997711181641</real> + </dict> + <dict> + <key>Bounds</key> + <string>{{67.130851632813631, 585.68960750345161}, {19.087577050122338, 39.91038694100309}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>549</integer> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.153172</string> + <key>g</key> + <string>0.153172</string> + <key>r</key> + <string>0.153172</string> + </dict> + <key>FillType</key> + <integer>2</integer> + <key>GradientAngle</key> + <real>145</real> + <key>GradientColor</key> + <dict> + <key>b</key> + <string>0.416928</string> + <key>g</key> + <string>0.416928</string> + <key>r</key> + <string>0.416928</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>CornerRadius</key> + <real>2</real> + </dict> + </dict> + </dict> + </array> + <key>ID</key> + <integer>539</integer> + </dict> + <dict> + <key>Bounds</key> + <string>{{64, 104}, {54, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>1</integer> + <key>Shape</key> + <string>Rectangle</string> + </dict> + <dict> + <key>Bounds</key> + <string>{{125.57142639160156, 24}, {36, 36}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>6</integer> + <key>Shape</key> + <string>Diamond</string> + <key>Style</key> + <dict/> + <key>Text</key> + <dict> + <key>VerticalPad</key> + <integer>0</integer> + </dict> + </dict> + <dict> + <key>Bounds</key> + <string>{{158.14285217276469, 695.84997558593727}, {44, 24}}</string> + <key>Class</key> + <string>ShapedGraphic</string> + <key>ID</key> + <integer>550</integer> + <key>Rotation</key> + <real>90</real> + <key>Shape</key> + <string>Rectangle</string> + <key>Style</key> + <dict> + <key>fill</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.243722</string> + <key>g</key> + <string>0.864482</string> + <key>r</key> + <string>1</string> + </dict> + </dict> + <key>stroke</key> + <dict> + <key>Color</key> + <dict> + <key>b</key> + <string>0.051159</string> + <key>g</key> + <string>0.160546</string> + <key>r</key> + <string>0.18663</string> + </dict> + <key>CornerRadius</key> + <real>5</real> + </dict> + </dict> + <key>Text</key> + <dict> + <key>Text</key> + <string>{\rtf1\ansi\ansicpg1252\cocoartf1187\cocoasubrtf370 +\cocoascreenfonts1{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural\qc + +\f0\fs24 \cf0 ESC}</string> + </dict> + </dict> + </array> + <key>GridInfo</key> + <dict/> + <key>GuidesLocked</key> + <string>NO</string> + <key>GuidesVisible</key> + <string>YES</string> + <key>HPages</key> + <integer>2</integer> + <key>ImageCounter</key> + <integer>1</integer> + <key>KeepToScale</key> + <false/> + <key>Layers</key> + <array> + <dict> + <key>Lock</key> + <string>NO</string> + <key>Name</key> + <string>Layer 1</string> + <key>Print</key> + <string>YES</string> + <key>View</key> + <string>YES</string> + </dict> + </array> + <key>LayoutInfo</key> + <dict> + <key>Animate</key> + <string>NO</string> + <key>circoMinDist</key> + <real>18</real> + <key>circoSeparation</key> + <real>0.0</real> + <key>layoutEngine</key> + <string>dot</string> + <key>neatoSeparation</key> + <real>0.0</real> + <key>twopiSeparation</key> + <real>0.0</real> + </dict> + <key>LinksVisible</key> + <string>NO</string> + <key>MagnetsVisible</key> + <string>NO</string> + <key>MasterSheets</key> + <array/> + <key>ModificationDate</key> + <string>2013-06-04 16:51:32 +0000</string> + <key>Modifier</key> + <string>Lorenz Meier</string> + <key>NotesVisible</key> + <string>NO</string> + <key>Orientation</key> + <integer>2</integer> + <key>OriginVisible</key> + <string>NO</string> + <key>PageBreaks</key> + <string>YES</string> + <key>PrintInfo</key> + <dict> + <key>NSBottomMargin</key> + <array> + <string>float</string> + <string>41</string> + </array> + <key>NSHorizonalPagination</key> + <array> + <string>coded</string> + <string>BAtzdHJlYW10eXBlZIHoA4QBQISEhAhOU051bWJlcgCEhAdOU1ZhbHVlAISECE5TT2JqZWN0AIWEASqEhAFxlwCG</string> + </array> + <key>NSLeftMargin</key> + <array> + <string>float</string> + <string>18</string> + </array> + <key>NSPaperSize</key> + <array> + <string>size</string> + <string>{595, 842}</string> + </array> + <key>NSPrintReverseOrientation</key> + <array> + <string>int</string> + <string>0</string> + </array> + <key>NSRightMargin</key> + <array> + <string>float</string> + <string>18</string> + </array> + <key>NSTopMargin</key> + <array> + <string>float</string> + <string>18</string> + </array> + </dict> + <key>PrintOnePage</key> + <false/> + <key>ReadOnly</key> + <string>NO</string> + <key>RowAlign</key> + <integer>1</integer> + <key>RowSpacing</key> + <real>36</real> + <key>SheetTitle</key> + <string>Canvas 1</string> + <key>SmartAlignmentGuidesActive</key> + <string>YES</string> + <key>SmartDistanceGuidesActive</key> + <string>YES</string> + <key>UniqueID</key> + <integer>1</integer> + <key>UseEntirePage</key> + <false/> + <key>VPages</key> + <integer>1</integer> + <key>WindowInfo</key> + <dict> + <key>CurrentSheet</key> + <integer>0</integer> + <key>ExpandedCanvases</key> + <array> + <dict> + <key>name</key> + <string>Canvas 1</string> + </dict> + </array> + <key>Frame</key> + <string>{{176, 63}, {1581, 1355}}</string> + <key>ListView</key> + <true/> + <key>OutlineWidth</key> + <integer>142</integer> + <key>RightSidebar</key> + <false/> + <key>ShowRuler</key> + <true/> + <key>Sidebar</key> + <true/> + <key>SidebarWidth</key> + <integer>120</integer> + <key>VisibleRegion</key> + <string>{{-164, -216}, {1446, 1216}}</string> + <key>Zoom</key> + <real>1</real> + <key>ZoomValues</key> + <array> + <array> + <string>Canvas 1</string> + <real>1</real> + <real>2</real> + </array> + </array> + </dict> +</dict> +</plist> diff --git a/Documentation/position_control.odg b/Documentation/position_control.odg Binary files differdeleted file mode 100644 index 5fb002c5e..000000000 --- a/Documentation/position_control.odg +++ /dev/null diff --git a/Documentation/position_control.png b/Documentation/position_control.png Binary files differdeleted file mode 100644 index d8d1a8b0c..000000000 --- a/Documentation/position_control.png +++ /dev/null diff --git a/Documentation/position_estimator_app.odg b/Documentation/position_estimator_app.odg Binary files differdeleted file mode 100644 index a6fc67373..000000000 --- a/Documentation/position_estimator_app.odg +++ /dev/null diff --git a/Documentation/position_estimator_app.pdf b/Documentation/position_estimator_app.pdf Binary files differdeleted file mode 100644 index bc07209ee..000000000 --- a/Documentation/position_estimator_app.pdf +++ /dev/null diff --git a/Documentation/position_estimator_app.png b/Documentation/position_estimator_app.png Binary files differdeleted file mode 100644 index 377549f63..000000000 --- a/Documentation/position_estimator_app.png +++ /dev/null diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg Binary files differindex e35a83372..a8a6f93f3 100644 --- a/Documentation/rc_mode_switch.odg +++ b/Documentation/rc_mode_switch.odg diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf Binary files differindex 823b1d868..3141eed7f 100644 --- a/Documentation/rc_mode_switch.pdf +++ b/Documentation/rc_mode_switch.pdf diff --git a/Documentation/state_machine.odg b/Documentation/state_machine.odg Binary files differdeleted file mode 100644 index 2f55a13dd..000000000 --- a/Documentation/state_machine.odg +++ /dev/null diff --git a/Documentation/state_machine.png b/Documentation/state_machine.png Binary files differdeleted file mode 100644 index 4daeddfc9..000000000 --- a/Documentation/state_machine.png +++ /dev/null diff --git a/Firmware.sublime-project b/Firmware.sublime-project index 72bacee9f..7292307d5 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -32,7 +32,8 @@ "settings": { "tab_size": 8, - "translate_tabs_to_spaces": false + "translate_tabs_to_spaces": false, + "highlight_line": true }, "build_systems": [ @@ -100,7 +100,7 @@ all: $(STAGED_FIRMWARES) # is taken care of. # $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 - @echo %% Copying $@ + @$(ECHO) %% Copying $@ $(Q) $(COPY) $< $@ $(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@) @@ -111,9 +111,9 @@ $(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4 $(BUILD_DIR)%.build/firmware.px4: config = $(patsubst $(BUILD_DIR)%.build/firmware.px4,%,$@) $(BUILD_DIR)%.build/firmware.px4: work_dir = $(BUILD_DIR)$(config).build/ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: - @echo %%%% - @echo %%%% Building $(config) in $(work_dir) - @echo %%%% + @$(ECHO) %%%% + @$(ECHO) %%%% Building $(config) in $(work_dir) + @$(ECHO) %%%% $(Q) mkdir -p $(work_dir) $(Q) make -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ @@ -132,8 +132,6 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: # XXX Should support fetching/unpacking from a separate directory to permit # downloads of the prebuilt archives as well... # -# XXX PX4IO configuration name is bad - NuttX configs should probably all be "px4" -# NUTTX_ARCHIVES = $(foreach board,$(BOARDS),$(ARCHIVE_DIR)$(board).export) .PHONY: archives archives: $(NUTTX_ARCHIVES) @@ -146,15 +144,54 @@ endif $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh -$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS) - @echo %% Configuring NuttX for $(board) +$(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) (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) export + @$(ECHO) %% Exporting NuttX for $(board) + $(Q) make -r -j1 -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)) + +# +# The user can run the NuttX 'menuconfig' tool for a single board configuration with +# make BOARDS=<boardname> menuconfig +# +ifeq ($(MAKECMDGOALS),menuconfig) +ifneq ($(words $(BOARDS)),1) +$(error BOARDS must specify exactly one board for the menuconfig goal) +endif +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) (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 + @$(ECHO) %% Saving configuration file + $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig +else +menuconfig: + @$(ECHO) "" + @$(ECHO) "The menuconfig goal must be invoked without any other goal being specified" + @$(ECHO) "" +endif + +$(NUTTX_SRC): + @$(ECHO) "" + @$(ECHO) "NuttX sources missing - clone https://github.com/PX4/NuttX.git and try again." + @$(ECHO) "" + +# +# Testing targets +# +testbuild: + $(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8) # # Cleanup targets. 'clean' should remove all built products and force @@ -176,40 +213,43 @@ distclean: clean # .PHONY: help help: - @echo "" - @echo " PX4 firmware builder" - @echo " ====================" - @echo "" - @echo " Available targets:" - @echo " ------------------" - @echo "" - @echo " archives" - @echo " Build the NuttX RTOS archives that are used by the firmware build." - @echo "" - @echo " all" - @echo " Build all firmware configs: $(CONFIGS)" - @echo " A limited set of configs can be built with CONFIGS=<list-of-configs>" - @echo "" + @$(ECHO) "" + @$(ECHO) " PX4 firmware builder" + @$(ECHO) " ====================" + @$(ECHO) "" + @$(ECHO) " Available targets:" + @$(ECHO) " ------------------" + @$(ECHO) "" + @$(ECHO) " archives" + @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." + @$(ECHO) "" + @$(ECHO) " all" + @$(ECHO) " Build all firmware configs: $(CONFIGS)" + @$(ECHO) " A limited set of configs can be built with CONFIGS=<list-of-configs>" + @$(ECHO) "" @for config in $(CONFIGS); do \ echo " $$config"; \ echo " Build just the $$config firmware configuration."; \ echo ""; \ done - @echo " clean" - @echo " Remove all firmware build pieces." - @echo "" - @echo " distclean" - @echo " Remove all compilation products, including NuttX RTOS archives." - @echo "" - @echo " upload" - @echo " When exactly one config is being built, add this target to upload the" - @echo " firmware to the board when the build is complete. Not supported for" - @echo " all configurations." - @echo "" - @echo " Common options:" - @echo " ---------------" - @echo "" - @echo " V=1" - @echo " If V is set, more verbose output is printed during the build. This can" - @echo " help when diagnosing issues with the build or toolchain." - @echo "" + @$(ECHO) " clean" + @$(ECHO) " Remove all firmware build pieces." + @$(ECHO) "" + @$(ECHO) " distclean" + @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." + @$(ECHO) "" + @$(ECHO) " upload" + @$(ECHO) " When exactly one config is being built, add this target to upload the" + @$(ECHO) " firmware to the board when the build is complete. Not supported for" + @$(ECHO) " all configurations." + @$(ECHO) "" + @$(ECHO) " testbuild" + @$(ECHO) " Perform a complete clean build of the entire tree." + @$(ECHO) "" + @$(ECHO) " Common options:" + @$(ECHO) " ---------------" + @$(ECHO) "" + @$(ECHO) " V=1" + @$(ECHO) " If V is set, more verbose output is printed during the build. This can" + @$(ECHO) " help when diagnosing issues with the build or toolchain." + @$(ECHO) "" diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x new file mode 100644 index 000000000..58a970eba --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x @@ -0,0 +1,107 @@ +#!nsh +# +# Flight startup script for PX4FMU with PWM outputs. +# + +# disable USB and autostart +set USB no +set MODE custom + +echo "[init] doing PX4FMU Quad startup..." + +# +# Start the ORB +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_ATT_D 0 + param set MC_YAWPOS_D 0 + param set MC_YAWPOS_I 0 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.08 + param set RC_SCALE_PITCH 1 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_YAW 3 + + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink +# +mavlink start -d /dev/ttyS0 -b 57600 +usleep 5000 + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start the commander. +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +echo "[init] starting PWM output" +fmu mode_pwm +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/02_io_quad_x b/ROMFS/px4fmu_common/init.d/02_io_quad_x new file mode 100644 index 000000000..c63e92f6d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/02_io_quad_x @@ -0,0 +1,101 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +pwm -u 400 -m 0xff +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..f55ac2ae3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR +++ b/ROMFS/px4fmu_common/init.d/08_ardrone diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..e7173f6e6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IOAR_PX4FLOW_example +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 new file mode 100644 index 000000000..4450eb50d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_io_f330 @@ -0,0 +1,116 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.005 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.1 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 4.5 + param set MC_RCLOSS_THR 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.3 + param set MC_YAWPOS_P 0.6 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.1 + + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start MAVLink (depends on orb) +# +mavlink start +usleep 5000 + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start +pwm -u 400 -m 0xff + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# This sets a PWM right after startup (regardless of safety button) +# +px4io idle 900 900 900 900 + +# +# The values are for spinning motors when armed using DJI ESCs +# +px4io min 1200 1200 1200 1200 + +# +# Upper limits could be higher, this is on the safe side +# +px4io max 1800 1800 1800 1800 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +multirotor_att_control start + +# +# Start logging +# +sdlog2 start -r 20 -a -b 16 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.PX4IO b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 925a5703e..5090b98a4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.PX4IO +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE camflyer +set MODE custom # # Start the ORB (first app to start) @@ -18,6 +21,16 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi # # Force some key parameters to sane values @@ -68,6 +81,10 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 # # Start the sensors (depends on orb, px4io) @@ -93,15 +110,12 @@ control_demo start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom new file mode 100644 index 000000000..5090b98a4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -0,0 +1,121 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Set actuator limit to 100 Hz update (50 Hz PWM) +px4io limit 100 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +control_demo start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD b/ROMFS/px4fmu_common/init.d/40_io_segway index 5f2de0d7e..5742d685a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.IO_QUAD +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,8 +1,11 @@ #!nsh +# +# Flight startup script for PX4FMU+PX4IO +# -# Disable USB and autostart +# disable USB and autostart set USB no -set MODE quad +set MODE custom # # Start the ORB (first app to start) @@ -18,13 +21,23 @@ if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi # # Force some key parameters to sane values # MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor # see https://pixhawk.ethz.ch/mavlink/ # -param set MAV_TYPE 2 +param set MAV_TYPE 10 # # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) @@ -68,6 +81,11 @@ px4io start # Allow PX4IO to recover from midair restarts. # this is very unlikely, but quite safe and robust. px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 # # Start the sensors (depends on orb, px4io) @@ -87,21 +105,18 @@ attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix -multirotor_att_control start +md25 start 3 0x58 +segway start # # Start logging # -#sdlog start -s 4 +sdlog2 start -r 50 -a -b 14 # # Start system state # if blinkm start then - echo "using BlinkM for state indication" blinkm systemstate -else - echo "no BlinkM found, OK." -fi
\ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x b/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x deleted file mode 100644 index 980197d68..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.FMU_quad_x +++ /dev/null @@ -1,67 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU with PWM outputs. -# - -# Disable the USB interface -set USB no - -# Disable autostarting other apps -set MODE custom - -echo "[init] doing PX4FMU Quad startup..." - -# -# Start the ORB -# -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] -then - param load /fs/microsd/params -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ -# -param set MAV_TYPE 2 - -# -# Start MAVLink -# -mavlink start -d /dev/ttyS0 -b 57600 -usleep 5000 - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -echo "[init] starting PWM output" -fmu mode_pwm -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Start attitude control -# -multirotor_att_control start - -echo "[init] startup done, exiting" -exit
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 62c7184b8..5e80ddc2f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -25,7 +25,7 @@ then else echo "using L3GD20 and LSM303D" l3gd20 start - lsm303 start + lsm303d start fi # @@ -34,9 +34,10 @@ fi # ALWAYS start this task before the # preflight_check. # -sensors start - -# -# Check sensors - run AFTER 'sensors start' -# -preflight_check
\ No newline at end of file +if sensors start +then + # + # Check sensors - run AFTER 'sensors start' + # + preflight_check & +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 31af3991a..5b1bd272e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,8 +5,55 @@ echo "Starting MAVLink on this USB console" +# Stop tone alarm +tone_alarm stop + # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +if mavlink stop +then + echo "stopped other MAVLink instance" +fi +sleep 2 +mavlink start -b 230400 -d /dev/ttyACM0 + +# Start the commander +if commander start +then + echo "Commander started" +fi + +# Start px4io if present +if px4io start +then + echo "PX4IO driver started" +else + if fmu mode_serial + then + echo "FMU driver started" + fi +fi + +# Start sensors +sh /etc/init.d/rc.sensors + +# Start one of the estimators +if attitude_estimator_ekf status +then + echo "multicopter att filter running" +else + if att_pos_estimator_ekf status + then + echo "fixedwing att filter running" + else + attitude_estimator_ekf start + fi +fi + +# Start GPS +if gps start +then + echo "GPS started" +fi echo "MAVLink started, exiting shell.." diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 22dec87cb..f0ee1a0c6 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -7,7 +7,6 @@ # can change this to prevent automatic startup of the flight script. # set MODE autostart -set USB autoconnect # # Try to mount the microSD card. @@ -42,31 +41,106 @@ then sh /fs/microsd/etc/rc.txt fi +# 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 "[init] USB interface connected" + fi + + echo "Running rc.APM" + # if APM startup is successful then nsh will exit + sh /etc/init.d/rc.APM +fi + +if [ $MODE == autostart ] +then + +# +# Start the ORB (first app to start) +# +uorb start + # -# Check for USB host +# Load microSD params # -if [ $USB != autoconnect ] +if ramtron start then - echo "[init] not connecting USB" + param select /ramtron/params + if [ -f /ramtron/params ] + then + param load /ramtron/params + fi else - if sercon + param select /fs/microsd/params + if [ -f /fs/microsd/params ] then - echo "[init] USB interface connected" + param load /fs/microsd/params + fi +fi + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + then + echo "No newer version, skipping upgrade." else - if [ -f /dev/ttyACM0 ] - echo "[init] NSH via USB" + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log else - echo "[init] No USB connected" + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log + echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" fi fi fi -# 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 ] +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +if param compare SYS_AUTOSTART 1 then - echo Running rc.APM - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM + sh /etc/init.d/01_fmu_quad_x +fi + +if param compare SYS_AUTOSTART 2 +then + sh /etc/init.d/02_io_quad_x +fi + +if param compare SYS_AUTOSTART 8 +then + sh /etc/init.d/08_ardrone +fi + +if param compare SYS_AUTOSTART 9 +then + sh /etc/init.d/09_ardrone_flow +fi + +if param compare SYS_AUTOSTART 10 +then + sh /etc/init.d/10_io_f330 +fi + +if param compare SYS_AUTOSTART 30 +then + sh /etc/init.d/30_io_camflyer +fi + +if param compare SYS_AUTOSTART 31 +then + sh /etc/init.d/31_io_phantom +fi + +# End of autostart fi diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip Binary files differnew file mode 100644 index 000000000..7cb837e56 --- /dev/null +++ b/ROMFS/px4fmu_common/logging/conv.zip diff --git a/Tools/README.txt b/Tools/README.txt new file mode 100644 index 000000000..abeb9a4c7 --- /dev/null +++ b/Tools/README.txt @@ -0,0 +1,13 @@ +====== PX4 LOG CONVERSION ====== + +On each log session (commonly started and stopped by arming and disarming the vehicle) a new file logxxx.bin is created. In many cases there will be only one logfile named log001.bin (only one flight). + +There are two conversion scripts in this ZIP file: + +logconv.m: This is a MATLAB script which will automatically convert and display the flight data with a GUI. If running this script, the second script can be ignored. + +sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run: + +python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n "" + +Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux.
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/Tools/logconv.m index 3750ddae2..c416b8095 100644 --- a/ROMFS/px4fmu_common/logging/logconv.m +++ b/Tools/logconv.m @@ -16,7 +16,7 @@ close all % ************************************************************************
% Set the path to your sysvector.bin file here
-filePath = 'sysvector.bin';
+filePath = 'log001.bin';
% Set the minimum and maximum times to plot here [in seconds]
mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start]
@@ -26,8 +26,8 @@ maxtime=0; %The maximum time/timestamp to display, as set by the user [0 bDisplayGPS=true;
%conversion factors
-fconv_gpsalt=1E-3; %[mm] to [m]
-fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg]
+fconv_gpsalt=1; %[mm] to [m]
+fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg]
fconv_timestamp=1E-6; % [microseconds] to [seconds]
% ************************************************************************
@@ -36,7 +36,7 @@ fconv_timestamp=1E-6; % [microseconds] to [seconds] ImportPX4LogData();
%Translate min and max plot times to indices
-time=double(sysvector.timestamp) .*fconv_timestamp;
+time=double(sysvector.TIME_StartTime) .*fconv_timestamp;
mintime_log=time(1); %The minimum time/timestamp found in the log
maxtime_log=time(end); %The maximum time/timestamp found in the log
CurTime=mintime_log; %The current time at which to draw the aircraft position
@@ -76,109 +76,48 @@ DrawCurrentAircraftState(); % Other firmware versions might require different import
% routines.
+%% ************************************************************************
+% IMPORTPX4LOGDATA (nested function)
+% ************************************************************************
+% Attention: This is the import routine for firmware from ca. 03/2013.
+% Other firmware versions might require different import
+% routines.
+
function ImportPX4LogData()
- % Work around a Matlab bug (not related to PX4)
- % where timestamps from 1.1.1970 do not allow to
- % read the file's size
- if ismac
- system('touch -t 201212121212.12 sysvector.bin');
- end
% ************************************************************************
% RETRIEVE SYSTEM VECTOR
% *************************************************************************
% //All measurements in NED frame
- %
- % uint64_t timestamp; //[us]
- % float gyro[3]; //[rad/s]
- % float accel[3]; //[m/s^2]
- % float mag[3]; //[gauss]
- % float baro; //pressure [millibar]
- % float baro_alt; //altitude above MSL [meter]
- % float baro_temp; //[degree celcius]
- % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
- % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
- % float vbat; //battery voltage in [volt]
- % float bat_current - current drawn from battery at this time instant
- % float bat_discharged - discharged energy in mAh
- % float adc[4]; //remaining auxiliary ADC ports [volt]
- % float local_position[3]; //tangent plane mapping into x,y,z [m]
- % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
- % float attitude[3]; //pitch, roll, yaw [rad]
- % float rotMatrix[9]; //unitvectors
- % float actuator_control[4]; //unitvector
- % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
- % float diff_pressure; - pressure difference in millibar
- % float ind_airspeed;
- % float true_airspeed;
-
- % Definition of the logged values
- logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
- logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
- logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
- logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
-
- % First get length of one line
- columns = length(logFormat);
- lineLength = 0;
-
- for i=1:columns
- lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array;
+
+ % Convert to CSV
+ %arg1 = 'log-fx61-20130721-2.bin';
+ arg1 = filePath;
+ delim = ',';
+ time_field = 'TIME';
+ data_file = 'data.csv';
+ csv_null = '';
+
+ if not(exist(data_file, 'file'))
+ s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) );
end
+ if exist(data_file, 'file')
- if exist(filePath, 'file')
-
- fileInfo = dir(filePath);
- fileSize = fileInfo.bytes;
-
- elements = int64(fileSize./(lineLength));
-
- fid = fopen(filePath, 'r');
- offset = 0;
- for i=1:columns
- % using fread with a skip speeds up the import drastically, do not
- % import the values one after the other
- sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(...
- fid, ...
- [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ...
- lineLength - logFormat{i}.bytes*logFormat{i}.array, ...
- logFormat{i}.machineformat) ...
- );
- offset = offset + logFormat{i}.bytes*logFormat{i}.array;
- fseek(fid, offset,'bof');
- end
+ %data = csvread(data_file);
+ sysvector = tdfread(data_file, ',');
% shot the flight time
- time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
- time_s = time_us*1e-6;
- time_m = time_s/60;
-
- % close the logfile
- fclose(fid);
+ time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1);
+ time_s = uint64(time_us*1e-6);
+ time_m = uint64(time_s/60);
+ time_s = time_s - time_m * 60;
+
+ disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]);
- disp(['end log2matlab conversion' char(10)]);
+ disp(['logfile conversion finished.' char(10)]);
else
- disp(['file: ' filePath ' does not exist' char(10)]);
+ disp(['file: ' data_file ' does not exist' char(10)]);
end
end
@@ -296,11 +235,11 @@ function DrawRawData() % ************************************************************************
figure(h.figures(2));
% Only plot GPS data if available
- if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS)
+ if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS)
%Draw data
- plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ...
- double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ...
- double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.');
+ plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ...
+ double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ...
+ double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.');
title(h.axes(1),'GPS Position Data(if available)');
xlabel(h.axes(1),'Latitude [deg]');
ylabel(h.axes(1),'Longitude [deg]');
@@ -315,19 +254,19 @@ function DrawRawData() % PLOT WINDOW 2: IMU, baro altitude
% ************************************************************************
figure(h.figures(3));
- plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:));
+ plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]);
title(h.axes(2),'Magnetometers [Gauss]');
legend(h.axes(2),'x','y','z');
- plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:));
+ plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]);
title(h.axes(3),'Accelerometers [m/s²]');
legend(h.axes(3),'x','y','z');
- plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:));
+ plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]);
title(h.axes(4),'Gyroscopes [rad/s]');
legend(h.axes(4),'x','y','z');
- plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue');
+ plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue');
if(bDisplayGPS)
hold on;
- plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red');
+ plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red');
hold off
legend('Barometric Altitude [m]','GPS Altitude [m]');
else
@@ -340,22 +279,22 @@ function DrawRawData() % ************************************************************************
figure(h.figures(4));
%Attitude Estimate
- plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159);
+ plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159);
title(h.axes(6),'Estimated attitude [deg]');
legend(h.axes(6),'roll','pitch','yaw');
%Actuator Controls
- plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:));
+ plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]);
title(h.axes(7),'Actuator control [-]');
- legend(h.axes(7),'0','1','2','3');
+ legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]');
%Actuator Controls
- plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8));
+ plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]);
title(h.axes(8),'Actuator PWM (raw-)outputs [µs]');
legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8');
set(h.axes(8), 'YLim',[800 2200]);
%Airspeeds
- plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime));
+ plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime));
hold on
- plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime));
+ plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime));
hold off
%add GPS total airspeed here
title(h.axes(9),'Airspeed [m/s]');
@@ -385,33 +324,43 @@ function DrawCurrentAircraftState() %**********************************************************************
% Current aircraft state label update
%**********************************************************************
- acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',...
- 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',...
- 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];
- acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),...
- ', y=',num2str(sysvector.mag(i,2)),...
- ', z=',num2str(sysvector.mag(i,3)),']'];
- acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),...
- ', y=',num2str(sysvector.accel(i,2)),...
- ', z=',num2str(sysvector.accel(i,3)),']'];
- acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),...
- ', y=',num2str(sysvector.gyro(i,2)),...
- ', z=',num2str(sysvector.gyro(i,3)),']'];
- acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]'];
- acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),...
- ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),...
- ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']'];
+ acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',...
+ 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',...
+ 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
+ acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),...
+ ', y=',num2str(sysvector.IMU_MagY(i)),...
+ ', z=',num2str(sysvector.IMU_MagZ(i)),']'];
+ acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),...
+ ', y=',num2str(sysvector.IMU_AccY(i)),...
+ ', z=',num2str(sysvector.IMU_AccZ(i)),']'];
+ acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),...
+ ', y=',num2str(sysvector.IMU_GyroY(i)),...
+ ', z=',num2str(sysvector.IMU_GyroZ(i)),']'];
+ acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]'];
+ acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),...
+ ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),...
+ ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']'];
acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:');
- for j=1:4
- acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),','];
- end
+ %for j=1:4
+ acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),','];
+ acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),','];
+ acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),','];
+ acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),','];
+ %end
acstate{7,:}=[acstate{7,:},']'];
acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:');
- for j=1:8
- acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),','];
- end
+ %for j=1:8
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),','];
+ acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),','];
+ %end
acstate{8,:}=[acstate{8,:},']'];
- acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']'];
+ acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']'];
set(h.edits.AircraftState,'String',acstate);
@@ -422,13 +371,13 @@ function DrawCurrentAircraftState() figure(h.figures(2));
hold on;
if(CurTime>mintime+1) %the +1 is only a small bugfix
- h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ...
- double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ...
- double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2);
+ h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ...
+ double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ...
+ double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2);
end;
hold off
%Plot current position
- newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt];
+ newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt];
if(numel(h.pathpoints)<=3) %empty path
h.pathpoints(1,1:3)=newpoint;
else %Not empty, append new point
@@ -443,8 +392,8 @@ function DrawCurrentAircraftState() if(isvalidhandle(h.markertext))
delete(h.markertext); %delete old text
end
- h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,...
- double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc);
+ h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,...
+ double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc);
set(h.edits.CurTime,'String',CurTime);
%**********************************************************************
@@ -549,11 +498,11 @@ end % FINDMINMAXINDICES (nested function)
% ************************************************************************
function [idxmin,idxmax] = FindMinMaxTimeIndices()
- for i=1:size(sysvector.timestamp,1)
+ for i=1:size(sysvector.TIME_StartTime,1)
if time(i)>=mintime; idxmin=i; break; end
end
- for i=1:size(sysvector.timestamp,1)
- if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end
+ for i=1:size(sysvector.TIME_StartTime,1)
+ if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end
if time(i)>=maxtime; idxmax=i; break; end
end
mintime=time(idxmin);
diff --git a/Tools/logconv.py b/Tools/logconv.py deleted file mode 100644 index c47d22a45..000000000 --- a/Tools/logconv.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -"""Convert binary log generated by sdlog to CSV format - -Usage: python logconv.py <log.bin>""" - -__author__ = "Anton Babushkin" -__version__ = "0.1" - -import struct, sys - -def _unpack_packet(data): - s = "" - s += "Q" #.timestamp = buf.raw.timestamp, - s += "fff" #.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - s += "fff" #.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - s += "fff" #.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - s += "f" #.baro = buf.raw.baro_pres_mbar, - s += "f" #.baro_alt = buf.raw.baro_alt_meter, - s += "f" #.baro_temp = buf.raw.baro_temp_celcius, - s += "ffff" #.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - s += "ffffffff" #.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]}, - s += "f" #.vbat = buf.batt.voltage_v, - s += "f" #.bat_current = buf.batt.current_a, - s += "f" #.bat_discharged = buf.batt.discharged_mah, - s += "ffff" #.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, - s += "fff" #.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - s += "iii" #.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - s += "fff" #.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - s += "fffffffff" #.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - s += "fff" #.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - s += "ffff" #.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - s += "ffffff" #.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - s += "f" #.diff_pressure = buf.diff_pres.differential_pressure_pa, - s += "f" #.ind_airspeed = buf.airspeed.indicated_airspeed_m_s, - s += "f" #.true_airspeed = buf.airspeed.true_airspeed_m_s - s += "iii" # to align to 280 - d = struct.unpack(s, data) - return d - -def _main(): - if len(sys.argv) < 2: - print "Usage:\npython logconv.py <log.bin>" - return - fn = sys.argv[1] - sysvector_size = 280 - f = open(fn, "r") - while True: - data = f.read(sysvector_size) - if len(data) < sysvector_size: - break - a = [] - for i in _unpack_packet(data): - a.append(str(i)) - print ";".join(a) - f.close() - -if __name__ == "__main__": - _main() diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 318f72971..7fefc5908 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -1,6 +1,8 @@ #!/usr/bin/env python -"""Dump binary log generated by sdlog2 or APM as CSV +from __future__ import print_function + +"""Dump binary log generated by PX4's sdlog2 or APM as CSV Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] @@ -21,6 +23,11 @@ __version__ = "1.2" import struct, sys +if sys.hexversion >= 0x030000F0: + runningPython3 = True +else: + runningPython3 = False + class SDLog2Parser: BLOCK_SIZE = 8192 MSG_HEADER_LEN = 3 @@ -55,6 +62,8 @@ class SDLog2Parser: __time_msg = None __debug_out = False __correct_errors = False + __file_name = None + __file = None def __init__(self): return @@ -63,7 +72,7 @@ class SDLog2Parser: self.__msg_descrs = {} # message descriptions by message type map self.__msg_labels = {} # message labels by message name map self.__msg_names = [] # message names in the same order as FORMAT messages - self.__buffer = "" # buffer for input binary data + self.__buffer = bytearray() # buffer for input binary data self.__ptr = 0 # read pointer in buffer self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label" self.__csv_data = {} # current values for all columns @@ -87,6 +96,14 @@ class SDLog2Parser: def setCorrectErrors(self, correct_errors): self.__correct_errors = correct_errors + + def setFileName(self, file_name): + self.__file_name = file_name + if file_name != None: + self.__file = open(file_name, 'w+') + else: + self.__file = None + def process(self, fn): self.reset() @@ -95,7 +112,7 @@ class SDLog2Parser: for msg_name, show_fields in self.__msg_filter: self.__msg_filter_map[msg_name] = show_fields first_data_msg = True - f = open(fn, "r") + f = open(fn, "rb") bytes_read = 0 while True: chunk = f.read(self.BLOCK_SIZE) @@ -104,15 +121,15 @@ class SDLog2Parser: self.__buffer = self.__buffer[self.__ptr:] + chunk self.__ptr = 0 while self.__bytesLeft() >= self.MSG_HEADER_LEN: - head1 = ord(self.__buffer[self.__ptr]) - head2 = ord(self.__buffer[self.__ptr+1]) + head1 = self.__buffer[self.__ptr] + head2 = self.__buffer[self.__ptr+1] if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2): if self.__correct_errors: self.__ptr += 1 continue else: raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) - msg_type = ord(self.__buffer[self.__ptr+2]) + msg_type = self.__buffer[self.__ptr+2] if msg_type == self.MSG_TYPE_FORMAT: # parse FORMAT message if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: @@ -154,10 +171,13 @@ class SDLog2Parser: show_fields = self.__msg_labels.get(msg_name, []) self.__msg_filter_map[msg_name] = show_fields for field in show_fields: - full_label = msg_name + "." + field + full_label = msg_name + "_" + field self.__csv_columns.append(full_label) self.__csv_data[full_label] = None - print self.__csv_delim.join(self.__csv_columns) + if self.__file != None: + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + else: + print(self.__csv_delim.join(self.__csv_columns)) def __printCSVRow(self): s = [] @@ -168,16 +188,28 @@ class SDLog2Parser: else: v = str(v) s.append(v) - print self.__csv_delim.join(s) + + if self.__file != None: + print(self.__csv_delim.join(s), file=self.__file) + else: + print(self.__csv_delim.join(s)) def __parseMsgDescr(self): - data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + if runningPython3: + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + else: + data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])) msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - msg_name = data[2].strip("\0") - msg_format = data[3].strip("\0") - msg_labels = data[4].strip("\0").split(",") + if runningPython3: + msg_name = str(data[2], 'ascii').strip("\0") + msg_format = str(data[3], 'ascii').strip("\0") + msg_labels = str(data[4], 'ascii').strip("\0").split(",") + else: + msg_name = str(data[2]).strip("\0") + msg_format = str(data[3]).strip("\0") + msg_labels = str(data[4]).strip("\0").split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -194,8 +226,8 @@ class SDLog2Parser: self.__msg_names.append(msg_name) if self.__debug_out: if self.__filterMsg(msg_name) != None: - print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( - msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults) + print("MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( + msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)) self.__ptr += self.MSG_FORMAT_PACKET_LEN def __parseMsg(self, msg_descr): @@ -205,8 +237,11 @@ class SDLog2Parser: self.__csv_updated = False show_fields = self.__filterMsg(msg_name) if (show_fields != None): - data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) - for i in xrange(len(data)): + if runningPython3: + data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) + else: + data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) + for i in range(len(data)): if type(data[i]) is str: data[i] = data[i].strip("\0") m = msg_mults[i] @@ -214,17 +249,17 @@ class SDLog2Parser: data[i] = data[i] * m if self.__debug_out: s = [] - for i in xrange(len(data)): + for i in range(len(data)): label = msg_labels[i] if show_fields == "*" or label in show_fields: s.append(label + "=" + str(data[i])) - print "MSG %s: %s" % (msg_name, ", ".join(s)) + print("MSG %s: %s" % (msg_name, ", ".join(s))) else: # update CSV data buffer - for i in xrange(len(data)): + for i in range(len(data)): label = msg_labels[i] if label in show_fields: - self.__csv_data[msg_name + "." + label] = data[i] + self.__csv_data[msg_name + "_" + label] = data[i] if self.__time_msg != None and msg_name != self.__time_msg: self.__csv_updated = True if self.__time_msg == None: @@ -233,13 +268,14 @@ class SDLog2Parser: def _main(): if len(sys.argv) < 2: - print "Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n" - print "\t-v\tUse plain debug output instead of CSV.\n" - print "\t-e\tRecover from errors.\n" - print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n" - print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n" - print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed." - print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n" + print("Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n") + print("\t-v\tUse plain debug output instead of CSV.\n") + print("\t-e\tRecover from errors.\n") + print("\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n") + print("\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n") + print("\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed.") + print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n") + print("\t-fPrint to file instead of stdout") return fn = sys.argv[1] debug_out = False @@ -247,7 +283,8 @@ def _main(): msg_filter = [] csv_null = "" csv_delim = "," - time_msg = None + time_msg = "TIME" + file_name = None opt = None for arg in sys.argv[2:]: if opt != None: @@ -257,9 +294,11 @@ def _main(): csv_null = arg elif opt == "t": time_msg = arg + elif opt == "f": + file_name = arg elif opt == "m": show_fields = "*" - a = arg.split(".") + a = arg.split("_") if len(a) > 1: show_fields = a[1].split(",") msg_filter.append((a[0], show_fields)) @@ -277,6 +316,8 @@ def _main(): opt = "m" elif arg == "-t": opt = "t" + elif arg == "-f": + opt = "f" if csv_delim == "\\t": csv_delim = "\t" @@ -285,6 +326,7 @@ def _main(): parser.setCSVNull(csv_null) parser.setMsgFilter(msg_filter) parser.setTimeMsg(time_msg) + parser.setFileName(file_name) parser.setDebugOut(debug_out) parser.setCorrectErrors(correct_errors) parser.process(fn) diff --git a/makefiles/board_px4fmu-v1.mk b/makefiles/board_px4fmu-v1.mk index 837069644..4d692e31a 100644 --- a/makefiles/board_px4fmu-v1.mk +++ b/makefiles/board_px4fmu-v1.mk @@ -6,5 +6,6 @@ # Configure the toolchain # CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = PX4FMU_V1 include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4io-v1.mk b/makefiles/board_px4io-v1.mk index b0eb2dae7..1872a4124 100644 --- a/makefiles/board_px4io-v1.mk +++ b/makefiles/board_px4io-v1.mk @@ -6,5 +6,6 @@ # Configure the toolchain # CONFIG_ARCH = CORTEXM3 +CONFIG_BOARD = PX4IO_V1 include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 213eb651b..40eebcb52 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -27,17 +27,21 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott_telemetry +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/mkblctrl MODULES += drivers/md25 +MODULES += drivers/airspeed MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed MODULES += modules/sensors # # System commands # MODULES += systemcmds/eeprom +MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c @@ -49,6 +53,7 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config # # General system control @@ -70,6 +75,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # +MODULES += modules/segway MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control @@ -89,6 +95,7 @@ MODULES += modules/sdlog2 MODULES += modules/systemlib MODULES += modules/systemlib/mixer MODULES += modules/mathlib +MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index f1c1b496a..ecff77db9 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -153,6 +153,7 @@ ifeq ($(BOARD_FILE),) $(error Config $(CONFIG) references board $(BOARD), but no board definition file found) endif export BOARD +export BOARD_FILE include $(BOARD_FILE) $(info % BOARD = $(BOARD)) @@ -385,7 +386,7 @@ define BUILTIN_DEF endef # Don't generate until modules have updated their command files -$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(BUILTIN_COMMAND_FILES) +$(BUILTIN_CSRC): $(GLOBAL_DEPS) $(MODULE_OBJS) $(MODULE_MKFILES) $(BUILTIN_COMMAND_FILES) @$(ECHO) "CMDS: $@" $(Q) $(ECHO) '/* builtin command list - automatically generated, do not edit */' > $@ $(Q) $(ECHO) '#include <nuttx/config.h>' >> $@ diff --git a/makefiles/module.mk b/makefiles/module.mk index 9e4cbafc9..9c1a828cc 100644 --- a/makefiles/module.mk +++ b/makefiles/module.mk @@ -98,6 +98,7 @@ # # CONFIG # BOARD +# BOARD_FILE # MODULE_WORK_DIR # MODULE_OBJ # MODULE_MK @@ -117,7 +118,7 @@ $(info %% MODULE_MK = $(MODULE_MK)) # # Get the board/toolchain config # -include $(PX4_MK_DIR)/board_$(BOARD).mk +include $(BOARD_FILE) # # Get the module's config diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 92461fafb..168e41a5c 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -65,6 +65,7 @@ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ export MKFW = $(PX4_BASE)/Tools/px_mkfw.py export UPLOADER = $(PX4_BASE)/Tools/px_uploader.py export COPY = cp +export COPYDIR = cp -Rf export REMOVE = rm -f export RMDIR = rm -rf export GENROMFS = genromfs diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 3f4d3371a..9fd2dd516 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -85,6 +85,13 @@ ifeq ($(ARCHCPUFLAGS),) $(error Must set CONFIG_ARCH to one of CORTEXM4F, CORTEXM4 or CORTEXM3) endif +# Set the board flags +# +ifeq ($(CONFIG_BOARD),) +$(error Board config does not define CONFIG_BOARD) +endif +ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) + # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ diff --git a/makefiles/upload.mk b/makefiles/upload.mk index a0671f6ad..226a22884 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -27,8 +27,6 @@ all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) -upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER) - $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h new file mode 100644 index 000000000..839631b3a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -0,0 +1,391 @@ +/************************************************************************************ + * configs/stm32f4discovery/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H +#define __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#ifndef __ASSEMBLY__ +# include <stdint.h> +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMU uses a 24MHz crystal connected to the HSE. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +//#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* LED definitions ******************************************************************/ +/* If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any + * way. The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * px4fmu-v1. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * + * Note that UART5 has no optional pinout, so it is not listed here. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 +#define GPIO_USART2_RTS GPIO_USART2_RTS_1 +#define GPIO_USART2_CTS GPIO_USART2_CTS_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* UART DMA configuration for USART1/6 */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * PWM + * + * Four PWM outputs can be configured on pins otherwise shared with + * USART2; two can take the flow control pins if they are not being used. + * + * Pins: + * + * CTS - PA0 - TIM2CH1 + * RTS - PA1 - TIM2CH2 + * TX - PA2 - TIM2CH3 + * RX - PA3 - TIM2CH4 + * + */ +#define GPIO_TIM2_CH1OUT GPIO_TIM2_CH1OUT_1 +#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 +#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 +#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 + +/* + * PPM + * + * PPM input is handled by the HRT timer. + */ +#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) + +/* + * CAN + * + * CAN2 is routed to the expansion connector. + */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 + +/* + * Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_MS5611 0x76 +#define PX4_I2C_OBDEV_EEPROM NOTDEFINED + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x1a + +/* + * SPI + * + * There are sensors on SPI1, and SPI3 is connected to the microSD slot. + */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_NSS GPIO_SPI3_NSS_2 + +/* SPI DMA configuration for SPI3 (microSD) */ +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2 +/* XXX since we allocate the HP work stack from CCM RAM on normal system startup, + SPI1 will never run in DMA mode - so we can just give it a random config here. + What we really need to do is to make DMA configurable per channel, and always + disable it for SPI1. */ +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 + +/* + * Use these in place of the spi_dev_e enumeration to + * select a specific SPI device on SPI1 + */ +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_ACCEL 2 +#define PX4_SPIDEV_MPU 3 + +/* + * Optional devices on IO's external port + */ +#define PX4_SPIDEV_ACCEL_MAG 2 + +/* + * Tone alarm output + */ +#define TONE_ALARM_TIMER 3 /* timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +/************************************************************************************ + * Name: stm32_ledinit, stm32_setled, and stm32_setleds + * + * Description: + * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If + * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to + * control the LEDs from user applications. + * + ************************************************************************************/ + +#ifndef CONFIG_ARCH_LEDS +EXTERN void stm32_ledinit(void); +EXTERN void stm32_setled(int led, bool ledon); +EXTERN void stm32_setleds(uint8_t ledset); +#endif + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __CONFIG_PX4FMU_V1_INCLUDE_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/px4fmu-v1/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs new file mode 100644 index 000000000..7b2ea703a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/px4fmu-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig new file mode 100644 index 000000000..5b91930c9 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -0,0 +1,964 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +# CONFIG_HOST_LINUX is not set +CONFIG_HOST_OSX=y +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +CONFIG_DEBUG_SYMBOLS=y + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +CONFIG_ARCH_CHIP_STM32F405RG=y +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +# CONFIG_ARCH_CHIP_STM32F427V is not set +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2C3=y +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +# CONFIG_STM32_SPI2 is not set +CONFIG_STM32_SPI3=y +CONFIG_STM32_SYSCFG=y +# CONFIG_STM32_TIM1 is not set +# CONFIG_STM32_TIM2 is not set +# CONFIG_STM32_TIM3 is not set +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_TIM6=y +CONFIG_STM32_TIM7=y +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM12=y +CONFIG_STM32_TIM13=y +CONFIG_STM32_TIM14=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +# CONFIG_STM32_USART3 is not set +# CONFIG_STM32_UART4 is not set +CONFIG_STM32_UART5=y +CONFIG_STM32_USART6=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +CONFIG_STM32_JTAG_FULL_ENABLE=y +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +# CONFIG_STM32_JTAG_SW_ENABLE is not set +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM10_PWM is not set +# CONFIG_STM32_TIM11_PWM is not set +# CONFIG_STM32_TIM12_PWM is not set +# CONFIG_STM32_TIM13_PWM is not set +# CONFIG_STM32_TIM14_PWM is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +# CONFIG_USART1_RXDMA is not set +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RXDMA is not set +# CONFIG_UART4_RXDMA is not set +# CONFIG_UART5_RS485 is not set +CONFIG_UART5_RXDMA=y +# CONFIG_USART6_RS485 is not set +CONFIG_USART6_RXDMA=y +# CONFIG_USART7_RXDMA is not set +# CONFIG_USART8_RXDMA is not set +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=196608 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=2048 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 +CONFIG_NSH_MMCSDSPIPORTNO=3 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +# CONFIG_DEV_CONSOLE is not set +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_DISABLE_OS_API=y +# CONFIG_DISABLE_CLOCK is not set +# CONFIG_DISABLE_POSIX_TIMERS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_DISABLE_SIGNALS is not set +# CONFIG_DISABLE_MQUEUE is not set +# CONFIG_DISABLE_ENVIRON is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_STREAMS=25 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +# CONFIG_MMCSD_MULTIBLOCK_DISABLE is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +CONFIG_MMCSD_SPI=y +CONFIG_MMCSD_SPICLOCK=24000000 +# CONFIG_MMCSD_SDIO is not set +# CONFIG_MTD is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_SERIAL_REMOVABLE=y +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART5=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_UART5_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +CONFIG_NO_SERIAL_CONSOLE=y + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y + +# +# UART5 Configuration +# +CONFIG_UART5_RXBUFSIZE=32 +CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_BITS=8 +CONFIG_UART5_PARITY=0 +CONFIG_UART5_2STOP=0 +# CONFIG_UART5_IFLOWCONTROL is not set +# CONFIG_UART5_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_BITS=8 +CONFIG_USART6_PARITY=0 +CONFIG_USART6_2STOP=0 +# CONFIG_USART6_IFLOWCONTROL is not set +# CONFIG_USART6_OFLOWCONTROL is not set +CONFIG_SERIAL_IFLOWCONTROL=y +CONFIG_SERIAL_OFLOWCONTROL=y +CONFIG_USBDEV=y + +# +# USB Device Controller Driver Options +# +# CONFIG_USBDEV_ISOCHRONOUS is not set +# CONFIG_USBDEV_DUALSPEED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set +# CONFIG_USBDEV_DMA is not set +# CONFIG_USBDEV_TRACE is not set + +# +# USB Device Class Driver Options +# +# CONFIG_USBDEV_COMPOSITE is not set +# CONFIG_PL2303 is not set +CONFIG_CDCACM=y +CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_EP0MAXPACKET=64 +CONFIG_CDCACM_EPINTIN=1 +CONFIG_CDCACM_EPINTIN_FSSIZE=64 +CONFIG_CDCACM_EPINTIN_HSSIZE=64 +CONFIG_CDCACM_EPBULKOUT=3 +CONFIG_CDCACM_EPBULKOUT_FSSIZE=64 +CONFIG_CDCACM_EPBULKOUT_HSSIZE=512 +CONFIG_CDCACM_EPBULKIN=2 +CONFIG_CDCACM_EPBULKIN_FSSIZE=64 +CONFIG_CDCACM_EPBULKIN_HSSIZE=512 +CONFIG_CDCACM_NWRREQS=4 +CONFIG_CDCACM_NRDREQS=4 +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_RXBUFSIZE=256 +CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0010 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.x" +# CONFIG_USBMSC is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG=y +CONFIG_SYSLOG_CHAR=y +CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +CONFIG_GRAN_SINGLE=y +CONFIG_GRAN_INTR=y + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2048 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +# CONFIG_C99_BOOL8 is not set +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +CONFIG_EXAMPLES_CDCACM=y +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +# CONFIG_EXAMPLES_HELLO is not set +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y +# CONFIG_NSH_USBCONSOLE is not set + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +# CONFIG_SYSTEM_I2CTOOL is not set + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/px4fmu-v1/nsh/setenv.sh b/nuttx-configs/px4fmu-v1/nsh/setenv.sh new file mode 100755 index 000000000..db372217c --- /dev/null +++ b/nuttx-configs/px4fmu-v1/nsh/setenv.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# configs/px4fmu-v1/usbnsh/setenv.sh +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This is the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This is the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# These are the Cygwin paths to the locations where I installed the Atollic +# toolchain under windows. You will also have to edit this if you install +# the Atollic toolchain in any other location. /usr/bin is added before +# the Atollic bin path because there is are binaries named gcc.exe and g++.exe +# at those locations as well. +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for ARM Pro 2.3.0/ARMTools/bin" +#export TOOLCHAIN_BIN="/usr/bin:/cygdrive/c/Program Files (x86)/Atollic/TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0/ARMTools/bin" + +# This is the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4fmu-v1/scripts/ld.script b/nuttx-configs/px4fmu-v1/scripts/ld.script new file mode 100644 index 000000000..ced5b21b7 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/scripts/ld.script @@ -0,0 +1,149 @@ +/**************************************************************************** + * configs/px4fmu-v1/scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of CCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4fmu-v1/src/Makefile b/nuttx-configs/px4fmu-v1/src/Makefile new file mode 100644 index 000000000..6ef8b7d6a --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu-v1/src/Makefile +# +# Copyright (C) 2013 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx-configs/px4io-v1/include/README.txt b/nuttx-configs/px4io-v1/include/README.txt new file mode 100755 index 000000000..2264a80aa --- /dev/null +++ b/nuttx-configs/px4io-v1/include/README.txt @@ -0,0 +1 @@ +This directory contains header files unique to the PX4IO board. diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h new file mode 100755 index 000000000..6503a94cd --- /dev/null +++ b/nuttx-configs/px4io-v1/include/board.h @@ -0,0 +1,168 @@ +/************************************************************************************ + * configs/px4io/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <gnutt@nuttx.org> + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> +#ifndef __ASSEMBLY__ +# include <stdint.h> +# include <stdbool.h> +#endif +#include <stm32_rcc.h> +#include <stm32_sdio.h> +#include <stm32.h> + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* On-board crystal frequency is 24MHz (HSE) */ + +#define STM32_BOARD_XTAL 24000000ul + +/* Use the HSE output as the system clock */ + +#define STM32_SYSCLK_SW RCC_CFGR_SW_HSE +#define STM32_SYSCLK_SWS RCC_CFGR_SWS_HSE +#define STM32_SYSCLK_FREQUENCY STM32_BOARD_XTAL + +/* AHB clock (HCLK) is SYSCLK (24MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB2 clock (PCLK2) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK +#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +#define STM32_APB2_CLKIN (STM32_PCLK2_FREQUENCY) /* Timers 2-4 */ + +/* APB2 timer 1 will receive PCLK2. */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 clock (PCLK1) is HCLK (24MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLK +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* All timers run off PCLK */ + +#define STM32_APB1_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) + +/* + * Some of the USART pins are not available; override the GPIO + * definitions with an invalid pin configuration. + */ +#undef GPIO_USART2_CTS +#define GPIO_USART2_CTS 0xffffffff +#undef GPIO_USART2_RTS +#define GPIO_USART2_RTS 0xffffffff +#undef GPIO_USART2_CK +#define GPIO_USART2_CK 0xffffffff +#undef GPIO_USART3_TX +#define GPIO_USART3_TX 0xffffffff +#undef GPIO_USART3_CK +#define GPIO_USART3_CK 0xffffffff +#undef GPIO_USART3_CTS +#define GPIO_USART3_CTS 0xffffffff +#undef GPIO_USART3_RTS +#define GPIO_USART3_RTS 0xffffffff + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ + +/* + * PPM + * + * PPM input is handled by the HRT timer. + * + * Pin is PA8, timer 1, channel 1 + */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs new file mode 100644 index 000000000..712631f47 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -0,0 +1,166 @@ +############################################################################ +# configs/px4io-v1/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m3 \ + -mthumb \ + -march=armv7-m + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/px4io-v1/nsh/appconfig b/nuttx-configs/px4io-v1/nsh/appconfig new file mode 100644 index 000000000..48a41bcdb --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/appconfig @@ -0,0 +1,32 @@ +############################################################################ +# +# 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. +# +############################################################################ diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig new file mode 100755 index 000000000..3785e0367 --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -0,0 +1,537 @@ +############################################################################ +# configs/px4io-v1/nsh/defconfig +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# +############################################################################ +# +# architecture selection +# +# CONFIG_ARCH - identifies the arch subdirectory and, hence, the +# processor architecture. +# CONFIG_ARCH_family - for use in C code. This identifies the +# particular chip family that the architecture is implemented +# in. +# CONFIG_ARCH_architecture - for use in C code. This identifies the +# specific architecture within the chip family. +# CONFIG_ARCH_CHIP - Identifies the arch/*/chip subdirectory +# CONFIG_ARCH_CHIP_name - For use in C code +# CONFIG_ARCH_BOARD - identifies the configs subdirectory and, hence, +# the board that supports the particular chip or SoC. +# CONFIG_ARCH_BOARD_name - for use in C code +# CONFIG_ENDIAN_BIG - define if big endian (default is little endian) +# CONFIG_BOARD_LOOPSPERMSEC - for delay loops +# CONFIG_DRAM_SIZE - Describes the installed DRAM. +# CONFIG_DRAM_START - The start address of DRAM (physical) +# CONFIG_ARCH_IRQPRIO - The ST32F100CB supports interrupt prioritization +# CONFIG_ARCH_INTERRUPTSTACK - This architecture supports an interrupt +# stack. If defined, this symbol is the size of the interrupt +# stack in bytes. If not defined, the user task stacks will be +# used during interrupt handling. +# CONFIG_ARCH_STACKDUMP - Do stack dumps after assertions +# CONFIG_ARCH_BOOTLOADER - Set if you are using a bootloader. +# CONFIG_ARCH_LEDS - Use LEDs to show state. Unique to board architecture. +# CONFIG_ARCH_BUTTONS - Enable support for buttons. Unique to board architecture. +# CONFIG_ARCH_CALIBRATION - Enables some build in instrumentation that +# cause a 100 second delay during boot-up. This 100 second delay +# serves no purpose other than it allows you to calibrate +# CONFIG_BOARD_LOOPSPERMSEC. You simply use a stop watch to measure +# the 100 second delay then adjust CONFIG_BOARD_LOOPSPERMSEC until +# the delay actually is 100 seconds. +# CONFIG_ARCH_DMA - Support DMA initialization +# +CONFIG_ARCH="arm" +CONFIG_ARCH_ARM=y +CONFIG_ARCH_CORTEXM3=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32F100C8=y +# +# Board Selection +# +CONFIG_ARCH_BOARD_PX4IO_V1=y +# CONFIG_ARCH_BOARD_CUSTOM is not set +CONFIG_ARCH_BOARD="px4io-v1" +CONFIG_BOARD_LOOPSPERMSEC=2000 +CONFIG_DRAM_SIZE=0x00002000 +CONFIG_DRAM_START=0x20000000 +CONFIG_ARCH_IRQPRIO=y +CONFIG_ARCH_INTERRUPTSTACK=n +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARCH_BOOTLOADER=n +CONFIG_ARCH_LEDS=n +CONFIG_ARCH_BUTTONS=n +CONFIG_ARCH_CALIBRATION=n +CONFIG_ARCH_DMA=y +CONFIG_ARCH_MATH_H=y + +CONFIG_ARMV7M_CMNVECTOR=y +# CONFIG_ARMV7M_STACKCHECK is not set + +# +# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): +# +# CONFIG_STM32_DFU - Use the DFU bootloader, not JTAG +# +# JTAG Enable options: +# +# CONFIG_STM32_JTAG_FULL_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE - Enables full SWJ (JTAG-DP + SW-DP) +# but without JNTRST. +# CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled +# +CONFIG_STM32_DFU=n +CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=n + +# +# Individual subsystems can be enabled: +# +# AHB: +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=n +CONFIG_STM32_CRC=n +# APB1: +# Timers 2,3 and 4 are owned by the PWM driver +CONFIG_STM32_TIM2=n +CONFIG_STM32_TIM3=n +CONFIG_STM32_TIM4=n +CONFIG_STM32_TIM5=n +CONFIG_STM32_TIM6=n +CONFIG_STM32_TIM7=n +CONFIG_STM32_WWDG=n +CONFIG_STM32_SPI2=n +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=n +CONFIG_STM32_BKP=n +CONFIG_STM32_PWR=n +CONFIG_STM32_DAC=n +# APB2: +# We use our own ADC driver, but leave this on for clocking purposes. +CONFIG_STM32_ADC1=y +CONFIG_STM32_ADC2=n +# TIM1 is owned by the HRT +CONFIG_STM32_TIM1=n +CONFIG_STM32_SPI1=n +CONFIG_STM32_TIM8=n +CONFIG_STM32_USART1=y +CONFIG_STM32_ADC3=n + + +# +# STM32F100 specific serial device driver settings +# +# CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the +# console and ttys0 (default is the USART1). +# CONFIG_USARTn_RXBUFSIZE - Characters are buffered as received. +# This specific the size of the receive buffer +# CONFIG_USARTn_TXBUFSIZE - Characters are buffered before +# being sent. This specific the size of the transmit buffer +# CONFIG_USARTn_BAUD - The configure BAUD of the UART. Must be +# CONFIG_USARTn_BITS - The number of bits. Must be either 7 or 8. +# CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity +# CONFIG_USARTn_2STOP - Two stop bits +# +CONFIG_SERIAL_TERMIOS=y +CONFIG_STANDARD_SERIAL=y + +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART2_SERIAL_CONSOLE=n +CONFIG_USART3_SERIAL_CONSOLE=n + +CONFIG_USART1_TXBUFSIZE=64 +CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART3_TXBUFSIZE=64 + +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=64 +CONFIG_USART3_RXBUFSIZE=64 + +CONFIG_USART1_BAUD=115200 +CONFIG_USART2_BAUD=115200 +CONFIG_USART3_BAUD=115200 + +CONFIG_USART1_BITS=8 +CONFIG_USART2_BITS=8 +CONFIG_USART3_BITS=8 + +CONFIG_USART1_PARITY=0 +CONFIG_USART2_PARITY=0 +CONFIG_USART3_PARITY=0 + +CONFIG_USART1_2STOP=0 +CONFIG_USART2_2STOP=0 +CONFIG_USART3_2STOP=0 + +CONFIG_USART1_RXDMA=y +SERIAL_HAVE_CONSOLE_DMA=y +# Conflicts with I2C1 DMA +CONFIG_USART2_RXDMA=n +CONFIG_USART3_RXDMA=y + +# +# General build options +# +# CONFIG_RRLOAD_BINARY - make the rrload binary format used with +# BSPs from www.ridgerun.com using the tools/mkimage.sh script +# CONFIG_INTELHEX_BINARY - make the Intel HEX binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_MOTOROLA_SREC - make the Motorola S-Record binary format +# used with many different loaders using the GNU objcopy program +# Should not be selected if you are not using the GNU toolchain. +# CONFIG_RAW_BINARY - make a raw binary format file used with many +# different loaders using the GNU objcopy program. This option +# should not be selected if you are not using the GNU toolchain. +# CONFIG_HAVE_LIBM - toolchain supports libm.a +# +CONFIG_RRLOAD_BINARY=n +CONFIG_INTELHEX_BINARY=n +CONFIG_MOTOROLA_SREC=n +CONFIG_RAW_BINARY=y +CONFIG_HAVE_LIBM=n + +# +# General OS setup +# +# CONFIG_APPS_DIR - Identifies the relative path to the directory +# that builds the application to link with NuttX. Default: ../apps +# CONFIG_DEBUG - enables built-in debug options +# CONFIG_DEBUG_VERBOSE - enables verbose debug output +# CONFIG_DEBUG_SYMBOLS - build without optimization and with +# debug symbols (needed for use with a debugger). +# CONFIG_HAVE_CXX - Enable support for C++ +# CONFIG_HAVE_CXXINITIALIZE - The platform-specific logic includes support +# for initialization of static C++ instances for this architecture +# and for the selected toolchain (via up_cxxinitialize()). +# CONFIG_MM_REGIONS - If the architecture includes multiple +# regions of memory to allocate from, this specifies the +# number of memory regions that the memory manager must +# handle and enables the API mm_addregion(start, end); +# CONFIG_ARCH_LOWPUTC - architecture supports low-level, boot +# time console output +# CONFIG_MSEC_PER_TICK - The default system timer is 100Hz +# or MSEC_PER_TICK=10. This setting may be defined to +# inform NuttX that the processor hardware is providing +# system timer interrupts at some interrupt interval other +# than 10 msec. +# CONFIG_RR_INTERVAL - The round robin timeslice will be set +# this number of milliseconds; Round robin scheduling can +# be disabled by setting this value to zero. +# CONFIG_SCHED_INSTRUMENTATION - enables instrumentation in +# scheduler to monitor system performance +# CONFIG_TASK_NAME_SIZE - Spcifies that maximum size of a +# task name to save in the TCB. Useful if scheduler +# instrumentation is selected. Set to zero to disable. +# CONFIG_START_YEAR, CONFIG_START_MONTH, CONFIG_START_DAY - +# Used to initialize the internal time logic. +# CONFIG_GREGORIAN_TIME - Enables Gregorian time conversions. +# You would only need this if you are concerned about accurate +# time conversions in the past or in the distant future. +# CONFIG_JULIAN_TIME - Enables Julian time conversions. You +# would only need this if you are concerned about accurate +# time conversion in the distand past. You must also define +# CONFIG_GREGORIAN_TIME in order to use Julian time. +# CONFIG_DEV_CONSOLE - Set if architecture-specific logic +# provides /dev/console. Enables stdout, stderr, stdin. +# CONFIG_DEV_LOWCONSOLE - Use the simple, low-level serial console +# driver (minimul support) +# CONFIG_MUTEX_TYPES: Set to enable support for recursive and +# errorcheck mutexes. Enables pthread_mutexattr_settype(). +# CONFIG_PRIORITY_INHERITANCE : Set to enable support for priority +# inheritance on mutexes and semaphores. +# CONFIG_SEM_PREALLOCHOLDERS: This setting is only used if priority +# inheritance is enabled. It defines the maximum number of +# different threads (minus one) that can take counts on a +# semaphore with priority inheritance support. This may be +# set to zero if priority inheritance is disabled OR if you +# are only using semaphores as mutexes (only one holder) OR +# if no more than two threads participate using a counting +# semaphore. +# CONFIG_SEM_NNESTPRIO. If priority inheritance is enabled, +# then this setting is the maximum number of higher priority +# threads (minus 1) than can be waiting for another thread +# to release a count on a semaphore. This value may be set +# to zero if no more than one thread is expected to wait for +# a semaphore. +# CONFIG_FDCLONE_DISABLE. Disable cloning of all file descriptors +# by task_create() when a new task is started. If set, all +# files/drivers will appear to be closed in the new task. +# CONFIG_FDCLONE_STDIO. Disable cloning of all but the first +# three file descriptors (stdin, stdout, stderr) by task_create() +# when a new task is started. If set, all files/drivers will +# appear to be closed in the new task except for stdin, stdout, +# and stderr. +# CONFIG_SDCLONE_DISABLE. Disable cloning of all socket +# desciptors by task_create() when a new task is started. If +# set, all sockets will appear to be closed in the new task. +# CONFIG_SCHED_WORKQUEUE. Create a dedicated "worker" thread to +# handle delayed processing from interrupt handlers. This feature +# is required for some drivers but, if there are not complaints, +# can be safely disabled. The worker thread also performs +# garbage collection -- completing any delayed memory deallocations +# from interrupt handlers. If the worker thread is disabled, +# then that clean will be performed by the IDLE thread instead +# (which runs at the lowest of priority and may not be appropriate +# if memory reclamation is of high priority). If CONFIG_SCHED_WORKQUEUE +# is enabled, then the following options can also be used: +# CONFIG_SCHED_WORKPRIORITY - The execution priority of the worker +# thread. Default: 50 +# CONFIG_SCHED_WORKPERIOD - How often the worker thread checks for +# work in units of microseconds. Default: 50*1000 (50 MS). +# CONFIG_SCHED_WORKSTACKSIZE - The stack size allocated for the worker +# thread. Default: CONFIG_IDLETHREAD_STACKSIZE. +# CONFIG_SIG_SIGWORK - The signal number that will be used to wake-up +# the worker thread. Default: 4 +# CONFIG_SCHED_WAITPID - Enable the waitpid() API +# CONFIG_SCHED_ATEXIT - Enabled the atexit() API +# +CONFIG_USER_ENTRYPOINT="user_start" +#CONFIG_APPS_DIR= +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_FS=n +CONFIG_DEBUG_GRAPHICS=n +CONFIG_DEBUG_LCD=n +CONFIG_DEBUG_USB=n +CONFIG_DEBUG_NET=n +CONFIG_DEBUG_RTC=n +CONFIG_DEBUG_ANALOG=n +CONFIG_DEBUG_PWM=n +CONFIG_DEBUG_CAN=n +CONFIG_DEBUG_I2C=n +CONFIG_DEBUG_INPUT=n + +CONFIG_MSEC_PER_TICK=1 +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_MM_REGIONS=1 +CONFIG_MM_SMALL=y +CONFIG_ARCH_LOWPUTC=y +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=n +CONFIG_TASK_NAME_SIZE=8 +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_GREGORIAN_TIME=n +CONFIG_JULIAN_TIME=n +# this eats ~1KiB of RAM ... work out why +CONFIG_DEV_CONSOLE=y +CONFIG_DEV_LOWCONSOLE=n +CONFIG_MUTEX_TYPES=n +CONFIG_PRIORITY_INHERITANCE=n +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=0 +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WORKQUEUE=n +CONFIG_SCHED_WORKPRIORITY=50 +CONFIG_SCHED_WORKPERIOD=50000 +CONFIG_SCHED_WORKSTACKSIZE=1024 +CONFIG_SIG_SIGWORK=4 +CONFIG_SCHED_WAITPID=n +CONFIG_SCHED_ATEXIT=n + +# +# The following can be used to disable categories of +# APIs supported by the OS. If the compiler supports +# weak functions, then it should not be necessary to +# disable functions unless you want to restrict usage +# of those APIs. +# +# There are certain dependency relationships in these +# features. +# +# o mq_notify logic depends on signals to awaken tasks +# waiting for queues to become full or empty. +# o pthread_condtimedwait() depends on signals to wake +# up waiting tasks. +# +CONFIG_DISABLE_CLOCK=n +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_DISABLE_SIGNALS=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_ENVIRON=y +CONFIG_DISABLE_POLL=y + +# +# Misc libc settings +# +# CONFIG_NOPRINTF_FIELDWIDTH - sprintf-related logic is a +# little smaller if we do not support fieldwidthes +# +CONFIG_NOPRINTF_FIELDWIDTH=n + +# +# Allow for architecture optimized implementations +# +# The architecture can provide optimized versions of the +# following to improve system performance +# +CONFIG_ARCH_MEMCPY=n +CONFIG_ARCH_MEMCMP=n +CONFIG_ARCH_MEMMOVE=n +CONFIG_ARCH_MEMSET=n +CONFIG_ARCH_STRCMP=n +CONFIG_ARCH_STRCPY=n +CONFIG_ARCH_STRNCPY=n +CONFIG_ARCH_STRLEN=n +CONFIG_ARCH_STRNLEN=n +CONFIG_ARCH_BZERO=n + +# +# Sizes of configurable things (0 disables) +# +# CONFIG_MAX_TASKS - The maximum number of simultaneously +# active tasks. This value must be a power of two. +# CONFIG_MAX_TASK_ARGS - This controls the maximum number of +# of parameters that a task may receive (i.e., maxmum value +# of 'argc') +# CONFIG_NPTHREAD_KEYS - The number of items of thread- +# specific data that can be retained +# CONFIG_NFILE_DESCRIPTORS - The maximum number of file +# descriptors (one for each open) +# CONFIG_NFILE_STREAMS - The maximum number of streams that +# can be fopen'ed +# CONFIG_NAME_MAX - The maximum size of a file name. +# CONFIG_STDIO_BUFFER_SIZE - Size of the buffer to allocate +# on fopen. (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_STDIO_LINEBUFFER - If standard C buffered I/O is enabled +# (CONFIG_STDIO_BUFFER_SIZE > 0), then this option may be added +# to force automatic, line-oriented flushing the output buffer +# for putc(), fputc(), putchar(), puts(), fputs(), printf(), +# fprintf(), and vfprintf(). When a newline is encountered in +# the output string, the output buffer will be flushed. This +# (slightly) increases the NuttX footprint but supports the kind +# of behavior that people expect for printf(). +# CONFIG_NUNGET_CHARS - Number of characters that can be +# buffered by ungetc() (Only if CONFIG_NFILE_STREAMS > 0) +# CONFIG_PREALLOC_MQ_MSGS - The number of pre-allocated message +# structures. The system manages a pool of preallocated +# message structures to minimize dynamic allocations +# CONFIG_MQ_MAXMSGSIZE - Message structures are allocated with +# a fixed payload size given by this settin (does not include +# other message structure overhead. +# CONFIG_MAX_WDOGPARMS - Maximum number of parameters that +# can be passed to a watchdog handler +# CONFIG_PREALLOC_WDOGS - The number of pre-allocated watchdog +# structures. The system manages a pool of preallocated +# watchdog structures to minimize dynamic allocations +# CONFIG_PREALLOC_TIMERS - The number of pre-allocated POSIX +# timer structures. The system manages a pool of preallocated +# timer structures to minimize dynamic allocations. Set to +# zero for all dynamic allocations. +# +CONFIG_MAX_TASKS=4 +CONFIG_MAX_TASK_ARGS=4 +CONFIG_NPTHREAD_KEYS=2 +CONFIG_NFILE_DESCRIPTORS=8 +CONFIG_NFILE_STREAMS=0 +CONFIG_NAME_MAX=12 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=n +CONFIG_NUNGET_CHARS=2 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=4 +CONFIG_PREALLOC_TIMERS=0 + + +# +# Settings for apps/nshlib +# +# CONFIG_NSH_BUILTIN_APPS - Support external registered, +# "named" applications that can be executed from the NSH +# command line (see apps/README.txt for more information). +# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer +# CONFIG_NSH_STRERROR - Use strerror(errno) +# CONFIG_NSH_LINELEN - Maximum length of one command line +# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi +# CONFIG_NSH_DISABLESCRIPT - Disable scripting support +# CONFIG_NSH_DISABLEBG - Disable background commands +# CONFIG_NSH_ROMFSETC - Use startup script in /etc +# CONFIG_NSH_CONSOLE - Use serial console front end +# CONFIG_NSH_TELNET - Use telnetd console front end +# CONFIG_NSH_ARCHINIT - Platform provides architecture +# specific initialization (nsh_archinitialize()). +# + +# Disable NSH completely +CONFIG_NSH_CONSOLE=n + +# +# Stack and heap information +# +# CONFIG_BOOT_RUNFROMFLASH - Some configurations support XIP +# operation from FLASH but must copy initialized .data sections to RAM. +# (should also be =n for the STM3210E-EVAL which always runs from flash) +# CONFIG_BOOT_COPYTORAM - Some configurations boot in FLASH +# but copy themselves entirely into RAM for better performance. +# CONFIG_CUSTOM_STACK - The up_ implementation will handle +# all stack operations outside of the nuttx model. +# CONFIG_STACK_POINTER - The initial stack pointer (arm7tdmi only) +# CONFIG_IDLETHREAD_STACKSIZE - The size of the initial stack. +# This is the thread that (1) performs the inital boot of the system up +# to the point where user_start() is spawned, and (2) there after is the +# IDLE thread that executes only when there is no other thread ready to +# run. +# CONFIG_USERMAIN_STACKSIZE - The size of the stack to allocate +# for the main user thread that begins at the user_start() entry point. +# CONFIG_PTHREAD_STACK_MIN - Minimum pthread stack size +# CONFIG_PTHREAD_STACK_DEFAULT - Default pthread stack size +# CONFIG_HEAP_BASE - The beginning of the heap +# CONFIG_HEAP_SIZE - The size of the heap +# +CONFIG_BOOT_RUNFROMFLASH=n +CONFIG_BOOT_COPYTORAM=n +CONFIG_CUSTOM_STACK=n +CONFIG_STACK_POINTER= +CONFIG_IDLETHREAD_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=1024 +CONFIG_HEAP_BASE= +CONFIG_HEAP_SIZE= + +# +# NSH Library +# +# CONFIG_NSH_LIBRARY is not set +# CONFIG_NSH_BUILTIN_APPS is not set diff --git a/nuttx-configs/px4io-v1/nsh/setenv.sh b/nuttx-configs/px4io-v1/nsh/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v1/nsh/setenv.sh @@ -0,0 +1,47 @@ +#!/bin/bash +# configs/stm3210e-eval/dfu/setenv.sh +# +# Copyright (C) 2009 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <gnutt@nuttx.org> +# +# 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 NuttX 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. +# + +if [ "$(basename $0)" = "setenv.sh" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then export PATH_ORIG="${PATH}"; fi + +WD=`pwd` +export RIDE_BIN="/cygdrive/c/Program Files/Raisonance/Ride/arm-gcc/bin" +export BUILDROOT_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" +export PATH="${BUILDROOT_BIN}:${RIDE_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/px4io-v1/scripts/ld.script b/nuttx-configs/px4io-v1/scripts/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v1/scripts/ld.script @@ -0,0 +1,129 @@ +/**************************************************************************** + * configs/stm3210e-eval/nsh/ld.script + * + * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt <spudmonkey@racsa.co.cr> + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F100C8 has 64Kb of FLASH beginning at address 0x0800:0000 and + * 8Kb of SRAM beginning at address 0x2000:0000. When booting from FLASH, + * FLASH memory is aliased to address 0x0000:0000 where the code expects to + * begin execution by jumping to the entry point in the 0x0800:0000 address + * range. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08001000, LENGTH = 60K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 8K +} + +OUTPUT_ARCH(arm) +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + /* The STM32F100CB has 8Kb of SRAM beginning at the following address */ + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/px4io-v1/src/Makefile b/nuttx-configs/px4io-v1/src/Makefile new file mode 100644 index 000000000..bb9539d16 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/stm3210e-eval/src/Makefile +# +# Copyright (C) 2009-2010 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt <spudmonkey@racsa.co.cr> +# +# 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 NuttX 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c + +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + @$(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + @touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp new file mode 100644 index 000000000..5a8157deb --- /dev/null +++ b/src/drivers/airspeed/airspeed.cpp @@ -0,0 +1,378 @@ +/**************************************************************************** + * + * 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 ets_airspeed.cpp + * @author Simon Wilks + * + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/airspeed.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> + +#include <drivers/drv_airspeed.h> +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/subsystem_info.h> + +#include <drivers/airspeed/airspeed.h> + +Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : + I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _diff_pres_offset(0.0f), + _airspeed_pub(-1), + _conversion_interval(conversion_interval), + _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +Airspeed::~Airspeed() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +Airspeed::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct differential_pressure_s[_num_reports]; + + for (unsigned i = 0; i < _num_reports; i++) + _reports[i].max_differential_pressure_pa = 0; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the airspeed topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + + if (_airspeed_pub < 0) + warnx("failed to create airspeed sensor object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +Airspeed::probe() +{ + return measure(); +} + +int +Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_conversion_interval); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_conversion_interval)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case AIRSPEEDIOCSSCALE: { + struct airspeed_scale *s = (struct airspeed_scale*)arg; + _diff_pres_offset = s->offset_pa; + return OK; + } + + case AIRSPEEDIOCGSCALE: { + struct airspeed_scale *s = (struct airspeed_scale*)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; + } + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +Airspeed::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct differential_pressure_s); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_conversion_interval); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +void +Airspeed::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +Airspeed::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +Airspeed::cycle_trampoline(void *arg) +{ + Airspeed *dev = (Airspeed *)arg; + + dev->cycle(); +} + +void +Airspeed::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + warnx("poll interval: %u ticks", _measure_ticks); + warnx("report queue: %u (%u/%u @ %p)", + _num_reports, _oldest_report, _next_report, _reports); +} diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h new file mode 100644 index 000000000..89dfb22d7 --- /dev/null +++ b/src/drivers/airspeed/airspeed.h @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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 airspeed.h + * @author Simon Wilks + * + * Generic driver for airspeed sensors connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/airspeed.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> + +#include <drivers/drv_airspeed.h> +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/subsystem_info.h> + +/* Default I2C bus */ +#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class __EXPORT Airspeed : public device::I2C +{ +public: + Airspeed(int bus, int address, unsigned conversion_interval); + virtual ~Airspeed(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + virtual void print_info(); + +protected: + virtual int probe(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle() = 0; + virtual int measure() = 0; + virtual int collect() = 0; + + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + differential_pressure_s *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + float _diff_pres_offset; + + orb_advert_t _airspeed_pub; + + unsigned _conversion_interval; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + + /** + * 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. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk new file mode 100644 index 000000000..4eef06161 --- /dev/null +++ b/src/drivers/airspeed/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the generic airspeed driver. +# + +SRCS = airspeed.cpp diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073..be8968a4e 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -109,7 +109,7 @@ int ar_multiplexing_init() { int fd; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { warn("GPIO: open fail"); diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c index 212a92cfa..36af2298c 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu/px4fmu_init.c @@ -194,7 +194,7 @@ __EXPORT int nsh_archinitialize(void) /* initial LED state */ drv_led_start(); led_off(LED_AMBER); - led_on(LED_BLUE); + led_off(LED_BLUE); /* Configure SPI-based devices */ diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 422321850..47ebcd40a 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -111,21 +111,21 @@ CDev::~CDev() int CDev::init() { - int ret = OK; - // base class init first - ret = Device::init(); + int ret = Device::init(); if (ret != OK) goto out; // now register the driver - ret = register_driver(_devname, &fops, 0666, (void *)this); + if (_devname != nullptr) { + ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) - goto out; + if (ret != OK) + goto out; - _registered = true; + _registered = true; + } out: return ret; @@ -395,4 +395,4 @@ cdev_poll(struct file *filp, struct pollfd *fds, bool setup) return cdev->poll(filp, fds, setup); } -} // namespace device
\ No newline at end of file +} // namespace device diff --git a/src/drivers/device/device.cpp b/src/drivers/device/device.cpp index 04a5222c3..c3ee77b1c 100644 --- a/src/drivers/device/device.cpp +++ b/src/drivers/device/device.cpp @@ -223,5 +223,22 @@ interrupt(int irq, void *context) return OK; } +int +Device::read(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::write(unsigned offset, void *data, unsigned count) +{ + return -ENODEV; +} + +int +Device::ioctl(unsigned operation, unsigned &arg) +{ + return -ENODEV; +} -} // namespace device
\ No newline at end of file +} // namespace device diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7d375aab9..a9ed5d77c 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -69,10 +69,61 @@ class __EXPORT Device { public: /** + * Destructor. + * + * Public so that anonymous devices can be destroyed. + */ + virtual ~Device(); + + /** * Interrupt handler. */ virtual void interrupt(void *ctx); /**< interrupt handler */ + /* + * Direct access methods. + */ + + /** + * Initialise the driver and make it ready for use. + * + * @return OK if the driver initialized OK, negative errno otherwise; + */ + virtual int init(); + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param offset The device address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + virtual int read(unsigned address, void *data, unsigned count); + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param address The device address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + virtual int write(unsigned address, void *data, unsigned count); + + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int ioctl(unsigned operation, unsigned &arg); + protected: const char *_name; /**< driver name */ bool _debug_enabled; /**< if true, debug messages are printed */ @@ -85,14 +136,6 @@ protected: */ Device(const char *name, int irq = 0); - ~Device(); - - /** - * Initialise the driver and make it ready for use. - * - * @return OK if the driver initialised OK. - */ - virtual int init(); /** * Enable the device interrupt @@ -189,7 +232,7 @@ public: /** * Destructor */ - ~CDev(); + virtual ~CDev(); virtual int init(); @@ -282,6 +325,7 @@ public: * Test whether the device is currently open. * * This can be used to avoid tearing down a device that is still active. + * Note - not virtual, cannot be overridden by a subclass. * * @return True if the device is currently open. */ @@ -396,9 +440,9 @@ public: const char *devname, uint32_t base, int irq = 0); - ~PIO(); + virtual ~PIO(); - int init(); + virtual int init(); protected: @@ -407,7 +451,7 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) { return *(volatile uint32_t *)(_base + offset); } @@ -444,4 +488,4 @@ private: } // namespace device -#endif /* _DEVICE_DEVICE_H */
\ No newline at end of file +#endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index b4a9cdd53..549879352 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -55,13 +55,11 @@ class __EXPORT I2C : public CDev public: - /** - * Get the address - */ - uint16_t get_address() { - return _address; - } - + /** + * Get the address + */ + int16_t get_address() { return _address; } + protected: /** * The number of times a read or write operation will be retried on @@ -90,7 +88,7 @@ protected: uint16_t address, uint32_t frequency, int irq = 0); - ~I2C(); + virtual ~I2C(); virtual int init(); diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h new file mode 100644 index 000000000..dc0c84052 --- /dev/null +++ b/src/drivers/device/ringbuffer.h @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 ringbuffer.h + * + * A simple ringbuffer template. + */ + +#pragma once + +template<typename T> +class RingBuffer { +public: + RingBuffer(unsigned size); + virtual ~RingBuffer(); + + /** + * Put an item into the buffer. + * + * @param val Item to put + * @return true if the item was put, false if the buffer is full + */ + bool put(T &val); + + /** + * Put an item into the buffer. + * + * @param val Item to put + * @return true if the item was put, false if the buffer is full + */ + bool put(const T &val); + + /** + * Get an item from the buffer. + * + * @param val Item that was gotten + * @return true if an item was got, false if the buffer was empty. + */ + bool get(T &val); + + /** + * Get an item from the buffer (scalars only). + * + * @return The value that was fetched, or zero if the buffer was + * empty. + */ + T get(void); + + /* + * Get the number of slots free in the buffer. + * + * @return The number of items that can be put into the buffer before + * it becomes full. + */ + unsigned space(void); + + /* + * Get the number of items in the buffer. + * + * @return The number of items that can be got from the buffer before + * it becomes empty. + */ + unsigned count(void); + + /* + * Returns true if the buffer is empty. + */ + bool empty() { return _tail == _head; } + + /* + * Returns true if the buffer is full. + */ + bool full() { return _next(_head) == _tail; } + + /* + * Returns the capacity of the buffer, or zero if the buffer could + * not be allocated. + */ + unsigned size() { return (_buf != nullptr) ? _size : 0; } + + /* + * Empties the buffer. + */ + void flush() { _head = _tail = _size; } + +private: + T *const _buf; + const unsigned _size; + volatile unsigned _head; /**< insertion point */ + volatile unsigned _tail; /**< removal point */ + + unsigned _next(unsigned index); +}; + +template <typename T> +RingBuffer<T>::RingBuffer(unsigned with_size) : + _buf(new T[with_size + 1]), + _size(with_size), + _head(with_size), + _tail(with_size) +{} + +template <typename T> +RingBuffer<T>::~RingBuffer() +{ + if (_buf != nullptr) + delete[] _buf; +} + +template <typename T> +bool RingBuffer<T>::put(T &val) +{ + unsigned next = _next(_head); + if (next != _tail) { + _buf[_head] = val; + _head = next; + return true; + } else { + return false; + } +} + +template <typename T> +bool RingBuffer<T>::put(const T &val) +{ + unsigned next = _next(_head); + if (next != _tail) { + _buf[_head] = val; + _head = next; + return true; + } else { + return false; + } +} + +template <typename T> +bool RingBuffer<T>::get(T &val) +{ + if (_tail != _head) { + val = _buf[_tail]; + _tail = _next(_tail); + return true; + } else { + return false; + } +} + +template <typename T> +T RingBuffer<T>::get(void) +{ + T val; + return get(val) ? val : 0; +} + +template <typename T> +unsigned RingBuffer<T>::space(void) +{ + return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); +} + +template <typename T> +unsigned RingBuffer<T>::count(void) +{ + return _size - space(); +} + +template <typename T> +unsigned RingBuffer<T>::_next(unsigned index) +{ + return (0 == index) ? _size : (index - 1); +} diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index d2d01efb3..e0122372a 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -71,7 +71,7 @@ protected: enum spi_mode_e mode, uint32_t frequency, int irq = 0); - ~SPI(); + virtual ~SPI(); virtual int init(); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index bffc35c62..7bb9ee2af 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -57,5 +57,14 @@ #define _AIRSPEEDIOCBASE (0x7700) #define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n)) +#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) +#define AIRSPEEDIOCGSCALE __AIRSPEEDIOC(1) + + +/** airspeed scaling factors; out = (in * Vscale) + offset */ +struct airspeed_scale { + float offset_pa; + float scale; +}; #endif /* _DRV_AIRSPEED_H */ diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 7e3998fca..510983d4b 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -63,43 +63,12 @@ * they also export GPIO-like things. This is always the GPIOs on the * main board. */ -# define GPIO_DEVICE_PATH "/dev/px4fmu" +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" - -#endif - -#ifndef GPIO_DEVICE_PATH +#ifndef PX4IO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 56af71059..ec9d4ca09 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -115,6 +115,12 @@ ORB_DECLARE(output_pwm); /** clear the 'ARM ok' bit, which deactivates the safety switch */ #define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +/** start DSM bind */ +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) + +/** Power up DSM receiver */ +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b34d3fa5d..cd72d9d23 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -72,9 +72,7 @@ #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/subsystem_info.h> - -/* Default I2C bus */ -#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION +#include <drivers/airspeed/airspeed.h> /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ @@ -91,336 +89,32 @@ /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ -/* Oddly, ERROR is not defined for C++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class ETSAirspeed : public device::I2C +class ETSAirspeed : public Airspeed { public: ETSAirspeed(int bus, int address = I2C_ADDRESS); - virtual ~ETSAirspeed(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); protected: - virtual int probe(); - -private: - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - int _diff_pres_offset; - - orb_advert_t _airspeed_pub; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - - /** - * 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. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - + virtual void cycle(); + virtual int measure(); + virtual int collect(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - /* * Driver 'main' command. */ extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); -ETSAirspeed::ETSAirspeed(int bus, int address) : - I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), - _num_reports(0), - _next_report(0), - _oldest_report(0), - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _diff_pres_offset(0), - _airspeed_pub(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")), - _comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows")) -{ - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); -} - -ETSAirspeed::~ETSAirspeed() +ETSAirspeed::ETSAirspeed(int bus, int address) : Airspeed(bus, address, + CONVERSION_INTERVAL) { - /* make sure we are truly inactive */ - stop(); - /* free any existing reports */ - if (_reports != nullptr) - delete[] _reports; -} - -int -ETSAirspeed::init() -{ - int ret = ERROR; - - /* do I2C init (and probe) first */ - if (I2C::init() != OK) - goto out; - - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct differential_pressure_s[_num_reports]; - - for (unsigned i = 0; i < _num_reports; i++) - _reports[i].max_differential_pressure_pa = 0; - - if (_reports == nullptr) - goto out; - - _oldest_report = _next_report = 0; - - /* get a publish handle on the airspeed topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); - - if (_airspeed_pub < 0) - debug("failed to create airspeed sensor object. Did you start uOrb?"); - - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; -out: - return ret; -} - -int -ETSAirspeed::probe() -{ - return measure(); -} - -int -ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; - - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } - } - - case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) - return SENSOR_POLLRATE_MANUAL; - - return (1000 / _measure_ticks); - - case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; - } - - case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; - - case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - -ssize_t -ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct differential_pressure_s); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) - return -ENOSPC; - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _oldest_report = _next_report = 0; - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - - } while (0); - - return ret; } int @@ -463,9 +157,15 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa = val[1] << 8 | val[0]; + if (diff_pres_pa == 0) { + // a zero value means the pressure sensor cannot give us a + // value. We need to return, and not report a value or the + // caller could end up using this value as part of an + // average + log("zero value from sensor"); + return -1; + } - // XXX move the parameter read out of the driver. - param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; @@ -506,47 +206,6 @@ ETSAirspeed::collect() } void -ETSAirspeed::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _oldest_report = _next_report = 0; - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_DIFFPRESSURE - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } -} - -void -ETSAirspeed::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -ETSAirspeed::cycle_trampoline(void *arg) -{ - ETSAirspeed *dev = (ETSAirspeed *)arg; - - dev->cycle(); -} - -void ETSAirspeed::cycle() { /* collection phase? */ @@ -571,7 +230,7 @@ ETSAirspeed::cycle() /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, - (worker_t)&ETSAirspeed::cycle_trampoline, + (worker_t)&Airspeed::cycle_trampoline, this, _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); @@ -589,22 +248,11 @@ ETSAirspeed::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, &_work, - (worker_t)&ETSAirspeed::cycle_trampoline, + (worker_t)&Airspeed::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL)); } -void -ETSAirspeed::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_buffer_overflows); - printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); -} - /** * Local functions in support of the shell command. */ @@ -642,7 +290,7 @@ start(int i2c_bus) if (g_dev == nullptr) goto fail; - if (OK != g_dev->init()) + if (OK != g_dev->Airspeed::init()) goto fail; /* set the poll rate to default, starts automatic data collection */ @@ -735,6 +383,10 @@ test() warnx("diff pressure: %d pa", report.differential_pressure_pa); } + /* reset the sensor polling to its default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + errx(1, "failed to set default rate"); + errx(0, "PASS"); } @@ -779,11 +431,11 @@ info() static void ets_airspeed_usage() { - fprintf(stderr, "usage: ets_airspeed command [options]\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT); - fprintf(stderr, "command:\n"); - fprintf(stderr, "\tstart|stop|reset|test|info\n"); + warnx("usage: ets_airspeed command [options]"); + warnx("options:"); + warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); + warnx("command:"); + warnx("\tstart|stop|reset|test|info"); } int diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index cb5d3b1ed..15346c5c5 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -36,6 +36,6 @@ # MODULE_COMMAND = ets_airspeed -MODULE_STACKSIZE = 1024 +MODULE_STACKSIZE = 2048 SRCS = ets_airspeed.cpp diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 762c257aa..b579db715 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * 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 @@ -55,13 +56,16 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 100 +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_waiting_for_ack(false), -_disable_cmd_counter(0) + _fd(fd), + _gps_position(gps_position), + _configured(false), + _waiting_for_ack(false), + _disable_cmd_counter(0) { decode_init(); } @@ -73,12 +77,13 @@ UBX::~UBX() int UBX::configure(unsigned &baudrate) { - _waiting_for_ack = true; - + _configured = false; /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < 5; baud_i++) { + int baud_i; + + for (baud_i = 0; baud_i < 5; baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); @@ -89,8 +94,8 @@ UBX::configure(unsigned &baudrate) /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_PRT; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_PRT; /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; @@ -102,9 +107,9 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -120,100 +125,125 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - receive(UBX_CONFIG_TIMEOUT); - + wait_for_ack(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + /* at this point we have correct baudrate on both ends */ + break; + } - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_RATE; + if (baud_i >= 5) { + return 1; + } - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + /* send a CFG-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_RATE; - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_NAV5; + send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: RATE"); + return 1; + } - send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, - // 0); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - - _waiting_for_ack = false; - return 0; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_NAV5; + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: NAV5"); + return 1; } - return -1; + + /* configure message rates */ + /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SOL\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + return 1; + } + + _configured = true; + return 0; } int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - int ret = receive(timeout); - _waiting_for_ack = false; - return ret; + uint64_t time_started = hrt_absolute_time(); + + while (hrt_absolute_time() < time_started + timeout * 1000) { + if (receive(timeout) > 0) { + if (!_waiting_for_ack) { + return 1; + } + + } else { + return -1; // timeout or error receiving, or NAK + } + } + + return -1; // timeout } int @@ -231,48 +261,47 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool position_updated = false; + bool handled = false; while (true) { - /* poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ return -1; } else if (ret == 0) { - /* Timeout */ - return -1; + /* return success after short delay after receiving a packet or timeout after long delay */ + return handled ? 1 : -1; } else if (ret > 0) { /* if we have new data from GPS, go handle it */ if (fds[0].revents & POLLIN) { /* * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If more bytes are available, we'll go back to poll() again. */ - count = ::read(_fd, buf, sizeof(buf)); - /* pass received bytes to the packet decoder */ + usleep(UBX_WAIT_BEFORE_READ * 1000); + count = read(_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i])) { + if (parse_char(buf[i]) > 0) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - if (handle_message()) - position_updated = true; + if (handle_message() > 0) + handled = true; } } } } - /* return success after receiving a packet */ - if (position_updated) - return 1; - - /* abort after timeout if no packet parsed successfully */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + /* abort after timeout if no useful packets received */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } } @@ -283,406 +312,292 @@ UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } - break; + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + + break; + /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - } else { - /* Second start symbol was wrong, reset state machine */ - decode_init(); - } - break; + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + /* don't return error, it can be just false sync1 */ + } + + break; + /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ - add_byte_to_checksum(b); - /* check for known class */ - switch (b) { - case UBX_CLASS_ACK: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = ACK; - break; + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + _message_class = b; + _decode_state = UBX_DECODE_GOT_CLASS; + break; - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + _message_id = b; + _decode_state = UBX_DECODE_GOT_MESSAGEID; + break; -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// break; + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; - case UBX_CLASS_CFG: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = CFG; - break; - default: //unknown class: reset state machine - decode_init(); - break; - } - break; - case UBX_DECODE_GOT_CLASS: - { + case UBX_DECODE_GOT_LENGTH1: + add_byte_to_checksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + + case UBX_DECODE_GOT_LENGTH2: + + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) add_byte_to_checksum(b); - switch (_message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_POSLLH; - break; - - case UBX_MESSAGE_NAV_SOL: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SOL; - break; - - case UBX_MESSAGE_NAV_TIMEUTC: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_TIMEUTC; - break; - -// case UBX_MESSAGE_NAV_DOP: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = NAV_DOP; -// break; - - case UBX_MESSAGE_NAV_SVINFO: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; -// case RXM: -// switch (b) { -// case UBX_MESSAGE_RXM_SVSI: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = RXM_SVSI; -// break; -// -// default: //unknown class: reset state machine, should not happen -// decode_init(); -// break; -// } -// break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know"); - decode_init(); -// config_needed = true; - break; - } - // Evaluate state machine - if the state changed, - // the state machine was reset via decode_init() - // and we want to tell the module to stop sending this message + _rx_buffer[_rx_count] = b; - // disable unknown message - //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - //configure_message_rate(_message_class, b, 0); + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + decode_init(); + return 1; // message received successfully + + } else { + warnx("ubx: checksum wrong"); + decode_init(); + return -1; } - break; - case UBX_DECODE_GOT_MESSAGEID: - add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - break; - case UBX_DECODE_GOT_LENGTH1: - add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - break; - case UBX_DECODE_GOT_LENGTH2: - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); - _rx_buffer[_rx_count] = b; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { - return 1; - } else { - decode_init(); - return -1; - warnx("ubx: Checksum wrong"); - } - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + + break; + + default: + break; } - return 0; //XXX ? + + return 0; // message decoding in progress } + int UBX::handle_message() { int ret = 0; - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + if (_configured) { + /* handle only info messages when configured */ + switch (_message_class) { + case UBX_CLASS_NAV: + switch (_message_id) { + case UBX_MESSAGE_NAV_POSLLH: { + // printf("GOT NAV_POSLLH\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->timestamp_position = hrt_absolute_time(); - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rate_count_lat_lon++; - _rate_count_lat_lon++; + ret = 1; + break; + } - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; - } - break; - } + case UBX_MESSAGE_NAV_SOL: { + // printf("GOT NAV_SOL\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + ret = 1; + break; + } - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } + case UBX_MESSAGE_NAV_TIMEUTC: { + // printf("GOT NAV_TIMEUTC\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// _gps_position->eph_m = packet->hDOP; -// _gps_position->epv = packet->vDOP; -// -// _gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + time_t epoch = mktime(&timeinfo); - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + _gps_position->timestamp_time = hrt_absolute_time(); - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; + ret = 1; + break; + } - time_t epoch = mktime(&timeinfo); + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + uint8_t satellites_used = 0; + int i; - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - case NAV_SVINFO: { - // printf("GOT NAV_SVINFO MESSAGE\n"); + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; - if (!_waiting_for_ack) { - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - // printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - /* Write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if ( sv_used ) { - // Count SVs used for NAV. - satellites_used++; + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); + } + + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; } - - // Record info for all channels, whether or not the SV is used for NAV. - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - } - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - } else { - _gps_position->satellite_info_available = false; + case UBX_MESSAGE_NAV_VELNED: { + // printf("GOT NAV_VELNED\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; + + ret = 1; + break; } - _gps_position->timestamp_satellites = hrt_absolute_time(); + + default: + break; } break; - } - case NAV_VELNED: { - - if (!_waiting_for_ack) { - /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); - - _rate_count_vel++; + case UBX_CLASS_ACK: { + /* ignore ACK when already configured */ + ret = 1; + break; } + default: break; } -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; -// -// _gps_position->satellites_visible = packet->numVis; -// _gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// -// break; -// } - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { - ret = 1; - } + if (ret == 0) { + /* message not handled */ + warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + + if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + /* don't attempt for every message to disable, some might not be disabled */ + warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + configure_message_rate(_message_class, _message_id, 0); } } - break; - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - ret = -1; - break; - } + } else { + /* handle only ACK while configuring */ + if (_message_class == UBX_CLASS_ACK) { + switch (_message_id) { + case UBX_MESSAGE_ACK_ACK: { + // printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { + _waiting_for_ack = false; + ret = 1; + } + } - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - if (_disable_cmd_counter++ == 0) { - // Don't attempt for every message to disable, some might not be disabled */ - warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); + break; + } + + case UBX_MESSAGE_ACK_NAK: { + // printf("GOT ACK_NAK\n"); + warnx("ubx: not acknowledged"); + /* configuration obviously not successful */ + _waiting_for_ack = false; + ret = -1; + break; + } + + default: + break; } - return ret; - ret = -1; - break; } - // end if _rx_count high enough + } + decode_init(); - return ret; //XXX? + return ret; } void @@ -692,9 +607,8 @@ UBX::decode_init(void) _rx_ck_b = 0; _rx_count = 0; _decode_state = UBX_DECODE_UNINIT; - _message_class = CLASS_UNKNOWN; - _message_id = ID_UNKNOWN; _payload_size = 0; + /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ } void @@ -705,23 +619,24 @@ UBX::add_byte_to_checksum(uint8_t b) } void -UBX::add_checksum_to_message(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t *message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; unsigned i; - for (i = 0; i < length-2; i++) { + for (i = 0; i < length - 2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ - message[length-2] = ck_a; - message[length-1] = ck_b; + message[length - 2] = ck_a; + message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) { for (unsigned i = 0; i < length; i++) { ck_a = ck_a + message[i]; @@ -732,11 +647,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_ void UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void @@ -761,19 +676,19 @@ void UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) { struct ubx_header header; - uint8_t ck_a=0, ck_b=0; + uint8_t ck_a = 0, ck_b = 0; header.sync1 = UBX_SYNC1; header.sync2 = UBX_SYNC2; header.msg_class = msg_class; header.msg_id = msg_id; header.length = size; - add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); // Configure receive check - _clsID_needed = msg_class; - _msgID_needed = msg_id; + _message_class_needed = msg_class; + _message_id_needed = msg_id; write(_fd, (const char *)&header, sizeof(header)); write(_fd, (const char *)msg, size); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5a433642c..4fc276975 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -3,6 +3,7 @@ * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * 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 @@ -51,23 +52,23 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 //#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 +#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ @@ -299,44 +300,6 @@ struct ubx_cfg_msg_rate { // ************ typedef enum { - UBX_CONFIG_STATE_PRT = 0, - UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, - UBX_CONFIG_STATE_RATE, - UBX_CONFIG_STATE_NAV5, - UBX_CONFIG_STATE_MSG_NAV_POSLLH, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, - UBX_CONFIG_STATE_MSG_NAV_DOP, - UBX_CONFIG_STATE_MSG_NAV_SVINFO, - UBX_CONFIG_STATE_MSG_NAV_SOL, - UBX_CONFIG_STATE_MSG_NAV_VELNED, -// UBX_CONFIG_STATE_MSG_RXM_SVSI, - UBX_CONFIG_STATE_CONFIGURED -} ubx_config_state_t; - -typedef enum { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -} ubx_message_class_t; - -typedef enum { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH, - NAV_SOL, - NAV_TIMEUTC, -// NAV_DOP, - NAV_SVINFO, - NAV_VELNED, -// RXM_SVSI, - CFG_NAV5, - ACK_ACK, - ACK_NAK, -} ubx_message_id_t; - -typedef enum { UBX_DECODE_UNINIT = 0, UBX_DECODE_GOT_SYNC1, UBX_DECODE_GOT_SYNC2, @@ -401,17 +364,17 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; - ubx_config_state_t _config_state; + bool _configured; bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + uint8_t _message_class_needed; + uint8_t _message_id_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - ubx_message_class_t _message_class; - ubx_message_id_t _message_id; + uint8_t _message_class; + uint8_t _message_id; unsigned _payload_size; uint8_t _disable_cmd_counter; }; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 59e90d86c..ac3bdc132 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1221,7 +1221,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver, attempt expansion bus first */ g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION); diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp new file mode 100644 index 000000000..cb8bbba37 --- /dev/null +++ b/src/drivers/hott/comms.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 comms.c + * @author Simon Wilks <sjwilks@gmail.com> + * + */ + +#include "comms.h" + +#include <fcntl.h> +#include <stdio.h> +#include <sys/ioctl.h> +#include <systemlib/err.h> +#include <termios.h> + +int +open_uart(const char *device) +{ + /* baud rate */ + static const speed_t speed = B19200; + + /* open uart */ + const int uart = open(device, O_RDWR | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", device); + } + + /* Back up the original uart configuration to restore it after exit */ + int termios_state; + struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { + close(uart); + err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + device, termios_state); + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + } + + /* Activate single wire mode */ + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + + return uart; +} diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h new file mode 100644 index 000000000..f5608122f --- /dev/null +++ b/src/drivers/hott/comms.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 comms.h + * @author Simon Wilks <sjwilks@gmail.com> + * + */ + + +#ifndef COMMS_H_ +#define COMMS_H + +int open_uart(const char *device); + +#endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp new file mode 100644 index 000000000..e322c6349 --- /dev/null +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.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, 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 hott_sensors.c + * @author Simon Wilks <sjwilks@gmail.com> + * + * Graupner HoTT sensor driver implementation. + * + * Poll any sensors connected to the PX4 via the telemetry wire. + */ + +#include <fcntl.h> +#include <nuttx/config.h> +#include <poll.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <sys/ioctl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include "../comms.h" +#include "../messages.h" + +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static const char daemon_name[] = "hott_sensors"; +static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]"; + +/** + * Deamon management function. + */ +extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int hott_sensors_thread_main(int argc, char *argv[]); + +static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id); +static int send_poll(int uart, uint8_t *buffer, size_t size); + +int +send_poll(int uart, uint8_t *buffer, size_t size) +{ + for (size_t i = 0; i < size; i++) { + write(uart, &buffer[i], sizeof(buffer[i])); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) +{ + static const int timeout_ms = 1000; + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; + + // XXX should this poll be inside the while loop??? + if (poll(&fds, 1, timeout_ms) > 0) { + int i = 0; + bool stop_byte_read = false; + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + + if (stop_byte_read) { + // XXX process checksum + *size = ++i; + return OK; + } + // XXX can some other field not have the STOP BYTE value? + if (buffer[i] == STOP_BYTE) { + *id = buffer[1]; + stop_byte_read = true; + } + i++; + } + } + return ERROR; +} + +int +hott_sensors_thread_main(int argc, char *argv[]) +{ + warnx("starting"); + + thread_running = true; + + const char *device = DEFAULT_UART; + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -d\n%s", commandline_usage); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + const int uart = open_uart(device); + if (uart < 0) { + errx(1, "Failed opening HoTT UART, exiting."); + thread_running = false; + } + + init_pub_messages(); + + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; + size_t size = 0; + uint8_t id = 0; + while (!thread_should_exit) { + // Currently we only support a General Air Module sensor. + build_gam_request(&buffer[0], &size); + send_poll(uart, buffer, size); + + // The sensor will need a little time before it starts sending. + usleep(5000); + + recv_data(uart, &buffer[0], &size, &id); + + // Determine which moduel sent it and process accordingly. + if (id == GAM_SENSOR_ID) { + publish_gam_message(buffer); + } else { + warnx("Unknown sensor ID: %d", id); + } + } + + warnx("exiting"); + close(uart); + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and start the daemon. + */ +int +hott_sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 1024, + hott_sensors_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("daemon is running"); + + } else { + warnx("daemon not started"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk new file mode 100644 index 000000000..b5f5762ba --- /dev/null +++ b/src/drivers/hott/hott_sensors/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# Graupner HoTT Sensors application. +# + +MODULE_COMMAND = hott_sensors + +SRCS = hott_sensors.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 4699ce5bf..042d9f816 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -41,7 +41,6 @@ * The HoTT receiver polls each device at a regular interval at which point * a data packet can be returned if necessary. * - * TODO: Add support for at least the vario and GPS sensor data. */ #include <fcntl.h> @@ -50,13 +49,21 @@ #include <stdlib.h> #include <stdio.h> #include <string.h> -#include <termios.h> #include <sys/ioctl.h> #include <unistd.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> -#include "messages.h" +#include "../comms.h" +#include "../messages.h" + +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ @@ -67,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop /** * Deamon management function. */ -__EXPORT int hott_telemetry_main(int argc, char *argv[]); +extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]); /** * Mainloop of daemon. @@ -77,60 +84,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]); static int recv_req_id(int uart, uint8_t *id); static int send_data(int uart, uint8_t *buffer, size_t size); -static int -open_uart(const char *device, struct termios *uart_config_original) -{ - /* baud rate */ - static const speed_t speed = B19200; - - /* open uart */ - const int uart = open(device, O_RDWR | O_NOCTTY); - - if (uart < 0) { - err(1, "Error opening port: %s", device); - } - - /* Back up the original uart configuration to restore it after exit */ - int termios_state; - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); - } - - /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); - } - - /* Activate single wire mode */ - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - - return uart; -} - int recv_req_id(int uart, uint8_t *id) { static const int timeout_ms = 1000; // TODO make it a define - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; uint8_t mode; + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; - if (poll(fds, 1, timeout_ms) > 0) { + if (poll(&fds, 1, timeout_ms) > 0) { /* Get the mode: binary or text */ read(uart, &mode, sizeof(mode)); @@ -155,7 +120,6 @@ send_data(int uart, uint8_t *buffer, size_t size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; - for (size_t i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -186,7 +150,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) thread_running = true; - const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ + const char *device = DEFAULT_UART; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { @@ -202,22 +166,20 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } - messages_init(); + init_sub_messages(); - uint8_t buffer[MESSAGE_BUFFER_SIZE]; + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; bool connected = true; - while (!thread_should_exit) { + // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { if (!connected) { connected = true; @@ -228,7 +190,9 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; - + case GAM_SENSOR_ID: + build_gam_response(buffer, &size); + break; case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; @@ -254,7 +218,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /** - * Process command line arguments and tart the daemon. + * Process command line arguments and start the daemon. */ int hott_telemetry_main(int argc, char *argv[]) diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index def1d59e9..b19cbd14c 100644 --- a/src/drivers/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -32,10 +32,11 @@ ############################################################################ # -# Graupner HoTT Telemetry application. +# Graupner HoTT Telemetry applications. # MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry_main.c \ - messages.c +SRCS = hott_telemetry.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott/messages.cpp index 0ce56acef..57c256339 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott/messages.cpp @@ -46,6 +46,7 @@ #include <unistd.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/esc_status.h> #include <uORB/topics/home_position.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_gps_position.h> @@ -53,24 +54,75 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -static int battery_sub = -1; -static int gps_sub = -1; -static int home_sub = -1; -static int sensor_sub = -1; -static int airspeed_sub = -1; +static int _battery_sub = -1; +static int _gps_sub = -1; +static int _home_sub = -1; +static int _sensor_sub = -1; +static int _airspeed_sub = -1; +static int _esc_sub = -1; -static bool home_position_set = false; -static double home_lat = 0.0d; -static double home_lon = 0.0d; +static orb_advert_t _esc_pub; +struct esc_status_s _esc; + +static bool _home_position_set = false; +static double _home_lat = 0.0d; +static double _home_lon = 0.0d; void -messages_init(void) +init_sub_messages(void) { - battery_sub = orb_subscribe(ORB_ID(battery_status)); - gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - home_sub = orb_subscribe(ORB_ID(home_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _home_sub = orb_subscribe(ORB_ID(home_position)); + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _esc_sub = orb_subscribe(ORB_ID(esc_status)); +} + +void +init_pub_messages(void) +{ + memset(&_esc, 0, sizeof(_esc)); + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); +} + +void +build_gam_request(uint8_t *buffer, size_t *size) +{ + struct gam_module_poll_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.mode = BINARY_MODE_REQUEST_ID; + msg.id = GAM_SENSOR_ID; + + memcpy(buffer, &msg, *size); +} + +void +publish_gam_message(const uint8_t *buffer) +{ + struct gam_module_msg msg; + size_t size = sizeof(msg); + memset(&msg, 0, size); + memcpy(&msg, buffer, size); + + /* announce the esc if needed, just publish else */ + if (_esc_pub > 0) { + orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + } else { + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + } + + // Publish it. + _esc.esc_count = 1; + _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + _esc.esc[0].esc_temperature = msg.temperature1 - 20; + _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); } void @@ -79,12 +131,12 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID(battery_status), _battery_sub, &battery); struct eam_module_msg msg; *size = sizeof(msg); @@ -92,7 +144,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; - msg.sensor_id = EAM_SENSOR_TEXT_ID; + msg.sensor_text_id = EAM_SENSOR_TEXT_ID; msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -106,12 +158,46 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the airspeed data */ struct airspeed_s airspeed; memset(&airspeed, 0, sizeof(airspeed)); - orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; - + + msg.stop = STOP_BYTE; + memcpy(buffer, &msg, *size); +} + +void +build_gam_response(uint8_t *buffer, size_t *size) +{ + /* get a local copy of the ESC Status values */ + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), _esc_sub, &esc); + + struct gam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.gam_sensor_id = GAM_SENSOR_ID; + msg.sensor_text_id = GAM_SENSOR_TEXT_ID; + + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature2 = 20; // 0 deg. C. + + uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + msg.main_voltage_L = (uint8_t)voltage & 0xff; + msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; + + uint16_t current = (uint16_t)(esc.esc[0].esc_current); + msg.current_L = (uint8_t)current & 0xff; + msg.current_H = (uint8_t)(current >> 8) & 0xff; + + uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + msg.rpm_L = (uint8_t)rpm & 0xff; + msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); @@ -123,14 +209,14 @@ build_gps_response(uint8_t *buffer, size_t *size) /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); - struct gps_module_msg msg = { 0 }; + struct gps_module_msg msg; *size = sizeof(msg); memset(&msg, 0, *size); @@ -150,7 +236,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); /* GPS speed */ - uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); + uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; @@ -196,33 +282,33 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); + uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - /* Get any (and probably only ever one) home_sub postion report */ + /* Get any (and probably only ever one) _home_sub postion report */ bool updated; - orb_check(home_sub, &updated); + orb_check(_home_sub, &updated); if (updated) { /* get a local copy of the home position data */ struct home_position_s home; memset(&home, 0, sizeof(home)); - orb_copy(ORB_ID(home_position), home_sub, &home); + orb_copy(ORB_ID(home_position), _home_sub, &home); - home_lat = ((double)(home.lat))*1e-7d; - home_lon = ((double)(home.lon))*1e-7d; - home_position_set = true; + _home_lat = ((double)(home.lat))*1e-7d; + _home_lon = ((double)(home.lon))*1e-7d; + _home_position_set = true; } /* Distance from home */ - if (home_position_set) { - uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + if (_home_position_set) { + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon); msg.distance_L = (uint8_t)dist & 0xff; msg.distance_H = (uint8_t)(dist >> 8) & 0xff; /* Direction back to home */ - uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); + uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F); msg.home_direction = (uint8_t)bearing >> 1; } } diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h new file mode 100644 index 000000000..224f8fc56 --- /dev/null +++ b/src/drivers/hott/messages.h @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.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, 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 messages.h + * @author Simon Wilks <sjwilks@gmail.com> + * + * Graupner HoTT Telemetry message generation. + * + */ +#ifndef MESSAGES_H_ +#define MESSAGES_H + +#include <stdlib.h> + +/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. + * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting + * the message after the read which takes some milliseconds. + */ +#define POST_READ_DELAY_IN_USECS 4000 +/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower + * values can be used in practise though. + */ +#define POST_WRITE_DELAY_IN_USECS 2000 + +// Protocol constants. +#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. +#define START_BYTE 0x7c +#define STOP_BYTE 0x7d +#define TEMP_ZERO_CELSIUS 0x14 + +/* The GAM Module poll message. */ +struct gam_module_poll_msg { + uint8_t mode; + uint8_t id; +}; + +/* Electric Air Module (EAM) constants. */ +#define EAM_SENSOR_ID 0x8e +#define EAM_SENSOR_TEXT_ID 0xe0 + +/* The Electric Air Module message. */ +struct eam_module_msg { + uint8_t start; /**< Start byte */ + uint8_t eam_sensor_id; /**< EAM sensor */ + uint8_t warning; + uint8_t sensor_text_id; + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2_L; + uint8_t cell3_L; + uint8_t cell4_L; + uint8_t cell5_L; + uint8_t cell6_L; + uint8_t cell7_L; + uint8_t cell1_H; + uint8_t cell2_H; + uint8_t cell3_H; + uint8_t cell4_H; + uint8_t cell5_H; + uint8_t cell6_H; + uint8_t cell7_H; + uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt1_voltage_H; + uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt2_voltage_H; + uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ + uint8_t temperature2; + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_H; + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ + uint8_t main_voltage_H; + uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ + uint8_t battery_capacity_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ + uint8_t climbrate_H; + uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_H; + uint8_t electric_min; /**< Flight time in minutes. */ + uint8_t electric_sec; /**< Flight time in seconds. */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_H; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ +}; + + +/* General Air Module (GAM) constants. */ +#define GAM_SENSOR_ID 0x8d +#define GAM_SENSOR_TEXT_ID 0xd0 + +struct gam_module_msg { + uint8_t start; /**< Start byte */ + uint8_t gam_sensor_id; /**< GAM sensor id */ + uint8_t warning_beeps; + uint8_t sensor_text_id; + uint8_t alarm_invers1; + uint8_t alarm_invers2; + uint8_t cell1; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt1_H; + uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt2_H; + uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ + uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ + uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ + /**< Graphical display ranges: 0 25% 50% 75% 100% */ + uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ + uint8_t fuel_ml_H; + uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm_H; + uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ + uint8_t altitude_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ + uint8_t climbrate_H; + uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ + uint8_t current_L; /**< Current in 0.1A steps */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ + uint8_t main_voltage_H; + uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ + uint8_t batt_cap_H; + uint8_t speed_L; /**< Speed in km/h */ + uint8_t speed_H; + uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ + uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ + uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm2_H; + uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ + uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ + uint8_t version; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed */ +}; + +/* GPS sensor constants. */ +#define GPS_SENSOR_ID 0x8a +#define GPS_SENSOR_TEXT_ID 0xa0 + +/** + * The GPS sensor message + * Struct based on: https://code.google.com/p/diy-hott-gps/downloads + */ +struct gps_module_msg { + uint8_t start; /**< Start byte */ + uint8_t sensor_id; /**< GPS sensor ID*/ + uint8_t warning; /**< 0…= warning beeps */ + uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ + uint8_t alarm_inverse1; /**< 01 inverse status */ + uint8_t alarm_inverse2; /**< 00 inverse status status 1 = no GPS Signal */ + uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ + uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */ + uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */ + + uint8_t latitude_ns; /**< 000 = N = 48°39’988 */ + uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */ + uint8_t latitude_min_H; /**< 018 18 = 0x12 */ + uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */ + uint8_t latitude_sec_H; /**< 016 3 = 0x03 */ + + uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */ + uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */ + uint8_t longitude_min_H; /**< 003 3 = 0x03 */ + uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */ + uint8_t longitude_sec_H; /**< 004 36 = 0x24 */ + + uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */ + uint8_t distance_H; /**< 036 35 = /distance high byte */ + uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */ + uint8_t altitude_H; /**< 001 1 = /Altitude high byte */ + uint8_t resolution_L; /**< 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ + uint8_t resolution_H; /**< 117 = High Byte m/s resolution 0.01m */ + uint8_t unknown1; /**< 120 = 0m/3s */ + uint8_t gps_num_sat; /**< GPS.Satellites (number of satelites) (1 byte) */ + uint8_t gps_fix_char; /**< GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ + uint8_t home_direction; /**< HomeDirection (direction from starting point to Model position) (1 byte) */ + uint8_t angle_x_direction; /**< angle x-direction (1 byte) */ + uint8_t angle_y_direction; /**< angle y-direction (1 byte) */ + uint8_t angle_z_direction; /**< angle z-direction (1 byte) */ + uint8_t gyro_x_L; /**< gyro x low byte (2 bytes) */ + uint8_t gyro_x_H; /**< gyro x high byte */ + uint8_t gyro_y_L; /**< gyro y low byte (2 bytes) */ + uint8_t gyro_y_H; /**< gyro y high byte */ + uint8_t gyro_z_L; /**< gyro z low byte (2 bytes) */ + uint8_t gyro_z_H; /**< gyro z high byte */ + uint8_t vibration; /**< vibration (1 bytes) */ + uint8_t ascii4; /**< 00 ASCII Free Character [4] */ + uint8_t ascii5; /**< 00 ASCII Free Character [5] */ + uint8_t gps_fix; /**< 00 ASCII Free Character [6], we use it for GPS FIX */ + uint8_t version; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed */ +}; + +// The maximum size of a message. +#define MAX_MESSAGE_BUFFER_SIZE 45 + +void init_sub_messages(void); +void init_pub_messages(void); +void build_gam_request(uint8_t *buffer, size_t *size); +void publish_gam_message(const uint8_t *buffer); +void build_eam_response(uint8_t *buffer, size_t *size); +void build_gam_response(uint8_t *buffer, size_t *size); +void build_gps_response(uint8_t *buffer, size_t *size); +float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); + + +#endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h deleted file mode 100644 index e6d5cc723..000000000 --- a/src/drivers/hott_telemetry/messages.h +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks <sjwilks@gmail.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, 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 messages.h - * @author Simon Wilks <sjwilks@gmail.com> - * - * Graupner HoTT Telemetry message generation. - * - */ -#ifndef MESSAGES_H_ -#define MESSAGES_H - -#include <stdlib.h> - -/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. - * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting - * the message after the read which takes some milliseconds. - */ -#define POST_READ_DELAY_IN_USECS 4000 -/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower - * values can be used in practise though. - */ -#define POST_WRITE_DELAY_IN_USECS 2000 - -// Protocol constants. -#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. -#define START_BYTE 0x7c -#define STOP_BYTE 0x7d -#define TEMP_ZERO_CELSIUS 0x14 - -/* Electric Air Module (EAM) constants. */ -#define EAM_SENSOR_ID 0x8e -#define EAM_SENSOR_TEXT_ID 0xe0 - -/* The Electric Air Module message. */ -struct eam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t eam_sensor_id; /**< EAM sensor */ - uint8_t warning; - uint8_t sensor_id; /**< Sensor ID, why different? */ - uint8_t alarm_inverse1; - uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2_L; - uint8_t cell3_L; - uint8_t cell4_L; - uint8_t cell5_L; - uint8_t cell6_L; - uint8_t cell7_L; - uint8_t cell1_H; - uint8_t cell2_H; - uint8_t cell3_H; - uint8_t cell4_H; - uint8_t cell5_H; - uint8_t cell6_H; - uint8_t cell7_H; - uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt1_voltage_H; - uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt2_voltage_H; - uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ - uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ - uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ - uint8_t main_voltage_H; - uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ - uint8_t battery_capacity_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ - uint8_t climbrate_H; - uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ - uint8_t rpm_H; - uint8_t electric_min; /**< Flight time in minutes. */ - uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ - uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ -}; - -/** - * The maximum buffer size required to store a HoTT message. - */ -#define MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ -}) - -/* GPS sensor constants. */ -#define GPS_SENSOR_ID 0x8A -#define GPS_SENSOR_TEXT_ID 0xA0 - -/** - * The GPS sensor message - * Struct based on: https://code.google.com/p/diy-hott-gps/downloads - */ -struct gps_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; /**< GPS sensor ID*/ - uint8_t warning; /**< Byte 3: 0…= warning beeps */ - uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ - uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ - uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ - uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ - uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */ - uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */ - - uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */ - uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */ - uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */ - uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */ - uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */ - - uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */ - uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */ - uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */ - uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/ - uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */ - - uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */ - uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */ - uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */ - uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */ - uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ - uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */ - uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */ - uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */ - uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ - uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */ - uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */ - uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */ - uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */ - uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */ - uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */ - uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */ - uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */ - uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */ - uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */ - uint8_t vibration; /**< Byte 39: vibration (1 bytes) */ - uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */ - uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */ - uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */ - uint8_t version; /**< Byte 43: 00 version number */ - uint8_t stop; /**< Byte 44: 0x7D Ende byte */ - uint8_t checksum; /**< Byte 45: Parity Byte */ -}; - -/** - * The maximum buffer size required to store a HoTT message. - */ -#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ -}) - -void messages_init(void); -void build_eam_response(uint8_t *buffer, size_t *size); -void build_gps_response(uint8_t *buffer, size_t *size); -float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); - - -#endif /* MESSAGES_H_ */ diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 98098c83b..744abfa00 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -260,6 +260,13 @@ private: * @return OK if the value can be supported. */ int set_samplerate(unsigned frequency); + + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); }; /* helper macro for handling report buffer indices */ @@ -333,8 +340,13 @@ L3GD20::init() write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); - write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */ + + write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ + + /* disable FIFO. This makes things simpler and ensures we + * aren't getting stale data. It means we must run the hrt + * callback fast enough to not miss data. */ + write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); set_range(500); /* default to 500dps */ set_samplerate(0); /* max sample rate */ @@ -514,6 +526,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGRANGE: return _current_range; + case GYROIOCSELFTEST: + return self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -708,7 +723,8 @@ L3GD20::measure() poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + if (_gyro_topic > 0) + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); /* stop the perf counter */ perf_end(_sample_perf); @@ -722,6 +738,28 @@ L3GD20::print_info() _num_reports, _oldest_report, _next_report, _reports); } +int +L3GD20::self_test() +{ + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + /** * Local functions in support of the shell command. */ diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp new file mode 100644 index 000000000..23b0724d8 --- /dev/null +++ b/src/drivers/md25/BlockSysIdent.cpp @@ -0,0 +1,8 @@ +#include "BlockSysIdent.hpp" + +BlockSysIdent::BlockSysIdent() : + Block(NULL, "SYSID"), + _freq(this, "FREQ"), + _ampl(this, "AMPL") +{ +} diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp new file mode 100644 index 000000000..270635f40 --- /dev/null +++ b/src/drivers/md25/BlockSysIdent.hpp @@ -0,0 +1,10 @@ +#include <controllib/block/Block.hpp> +#include <controllib/block/BlockParam.hpp> + +class BlockSysIdent : public control::Block { +public: + BlockSysIdent(); +private: + control::BlockParam<float> _freq; + control::BlockParam<float> _ampl; +}; diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 71932ad65..d43e3aef9 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -45,9 +45,16 @@ #include "md25.hpp" #include <poll.h> #include <stdio.h> +#include <math.h> +#include <string.h> #include <systemlib/err.h> #include <arch/board/board.h> +#include <mavlink/mavlink_log.h> + +#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/topics/debug_key_value.h> +#include <drivers/drv_hrt.h> // registers enum { @@ -72,6 +79,9 @@ enum { REG_COMMAND_RW, }; +// File descriptors +static int mavlink_fd; + MD25::MD25(const char *deviceName, int bus, uint16_t address, uint32_t speed) : I2C("MD25", deviceName, bus, address, speed), @@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus, setMotor2Speed(0); resetEncoders(); _setMode(MD25::MODE_UNSIGNED_SPEED); - setSpeedRegulation(true); + setSpeedRegulation(false); + setMotorAccel(10); setTimeout(true); } @@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address) return OK; } +int MD25::setMotorAccel(uint8_t accel) +{ + return _writeUint8(REG_ACCEL_RATE_RW, + accel); +} + int MD25::setMotor1Speed(float value) { return _writeUint8(REG_SPEED1_RW, @@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.1; float speed = 0.2; @@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) return 0; } +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency) +{ + printf("md25 sine: starting\n"); + + // setup + MD25 md25("/dev/md25", bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + // setup for test + md25.setSpeedRegulation(false); + md25.setTimeout(true); + float dt = 0.01; + float t_final = 60.0; + float prev_revolution = md25.getRevolutions1(); + + // debug publication + control::UOrbPublication<debug_key_value_s> debug_msg(NULL, + ORB_ID(debug_key_value)); + + // sine wave for motor 1 + md25.resetEncoders(); + while (true) { + + // input + uint64_t timestamp = hrt_absolute_time(); + float t = timestamp/1000000.0f; + + float input_value = amplitude*sinf(2*M_PI*frequency*t); + md25.setMotor1Speed(input_value); + + // output + md25.readData(); + float current_revolution = md25.getRevolutions1(); + + // send input message + //strncpy(debug_msg.key, "md25 in ", 10); + //debug_msg.timestamp_ms = 1000*timestamp; + //debug_msg.value = input_value; + //debug_msg.update(); + + // send output message + strncpy(debug_msg.key, "md25 out ", 10); + debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.value = current_revolution; + debug_msg.update(); + + if (t > t_final) break; + + // update for next step + prev_revolution = current_revolution; + + // sleep + usleep(1000000 * dt); + } + md25.setMotor1Speed(0); + + printf("md25 sine complete\n"); + return 0; +} + // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index e77511b16..1661f67f9 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/block/UOrbSubscription.hpp> +#include <controllib/uorb/UOrbSubscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -213,6 +213,19 @@ public: int setDeviceAddress(uint8_t address); /** + * set motor acceleration + * @param accel + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + int setMotorAccel(uint8_t accel); + + /** * set motor 1 speed * @param normSpeed normalize speed between -1 and 1 * @return non-zero -> error @@ -290,4 +303,7 @@ private: // unit testing int md25Test(const char *deviceName, uint8_t bus, uint8_t address); +// sine testing +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency); + // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index e62c46b0d..7e5904d05 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -82,7 +82,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); } @@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "sine")) { + + if (argc < 6) { + printf("usage: md25 sine bus address amp freq\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + float amplitude = atof(argv[4]); + + float frequency = atof(argv[5]); + + md25Sine(deviceName, bus, address, amplitude, frequency); + + exit(0); + } + if (!strcmp(argv[1], "probe")) { if (argc < 4) { printf("usage: md25 probe bus address\n"); @@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "read")) { + if (argc < 4) { + printf("usage: md25 read bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + exit(0); + } + + if (!strcmp(argv[1], "search")) { if (argc < 3) { printf("usage: md25 search bus\n"); @@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[]) uint8_t address = strtoul(argv[4], nullptr, 0); // start - MD25 md25("/dev/md25", bus, address); + MD25 md25(deviceName, bus, address); thread_running = true; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp new file mode 100644 index 000000000..68d2c5d65 --- /dev/null +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -0,0 +1,520 @@ +/**************************************************************************** + * + * 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 meas_airspeed.cpp + * @author Lorenz Meier + * @author Sarthak Kaingade + * @author Simon Wilks + * + * Driver for the MEAS Spec series connected via I2C. + * + * Supported sensors: + * + * - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf) + * - untested: MS5525DSO (http://www.meas-spec.com/downloads/MS5525DSO.pdf) + * + * Interface application notes: + * + * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/airspeed.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> + +#include <drivers/drv_airspeed.h> +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/subsystem_info.h> + +#include <drivers/airspeed/airspeed.h> + +/* I2C bus address is 1010001x */ +#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ +#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +class MEASAirspeed : public Airspeed +{ +public: + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + +protected: + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + virtual void cycle(); + virtual int measure(); + virtual int collect(); + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); + +MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, + CONVERSION_INTERVAL) +{ + +} + +int +MEASAirspeed::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = 0; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +MEASAirspeed::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[4] = {0, 0, 0, 0}; + + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 4); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + return ret; + } + + uint8_t status = val[0] & 0xC0; + + if (status == 2) { + log("err: stale data"); + + } else if (status == 3) { + log("err: fault"); + } + + //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); + uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; + + // XXX leaving this in until new calculation method has been cross-checked + //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); + //diff_pres_pa -= _diff_pres_offset; + int16_t dp_raw = 0, dT_raw = 0; + dp_raw = (val[0] << 8) + val[1]; + dp_raw = 0x3FFF & dp_raw; + dT_raw = (val[2] << 8) + val[3]; + dT_raw = (0xFFE0 & dT_raw) >> 5; + float temperature = ((200 * dT_raw) / 2047) - 50; + + // XXX we may want to smooth out the readings to remove noise. + + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative, enforce absolute value + uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].temperature = temperature; + _reports[_next_report].differential_pressure_pa = diff_press_pa; + + // Track maximum differential pressure measured (so we can work out top speed). + if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { + _reports[_next_report].max_differential_pressure_pa = diff_press_pa; + } + + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + + return ret; +} + +void +MEASAirspeed::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&Airspeed::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&Airspeed::cycle_trampoline, + this, + USEC2TICK(CONVERSION_INTERVAL)); +} + +/** + * Local functions in support of the shell command. + */ +namespace meas_airspeed +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MEASAirspeed *g_dev = nullptr; + +void start(int i2c_bus); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start(int i2c_bus) +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver, try the MS4525DO first */ + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + + /* check if the MS4525DO was instantiated */ + if (g_dev == nullptr) + goto fail; + + /* try the MS5525DSO next if init fails */ + if (OK != g_dev->Airspeed::init()) { + delete g_dev; + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + + /* check if the MS5525DSO was instantiated */ + if (g_dev == nullptr) + goto fail; + + /* both versions failed if the init for the MS5525DSO fails, give up */ + if (OK != g_dev->Airspeed::init()) + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void +stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct differential_pressure_s report; + ssize_t sz; + int ret; + + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("diff pressure: %d pa", report.differential_pressure_pa); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); + } + + /* reset the sensor polling to its default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + errx(1, "failed to set default rate"); + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +static void +meas_airspeed_usage() +{ + warnx("usage: meas_airspeed command [options]"); + warnx("options:"); + warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT); + warnx("command:"); + warnx("\tstart|stop|reset|test|info"); +} + +int +meas_airspeed_main(int argc, char *argv[]) +{ + int i2c_bus = PX4_I2C_BUS_DEFAULT; + + int i; + + for (i = 1; i < argc; i++) { + if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { + if (argc > i + 1) { + i2c_bus = atoi(argv[i + 1]); + } + } + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + meas_airspeed::start(i2c_bus); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + meas_airspeed::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + meas_airspeed::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + meas_airspeed::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + meas_airspeed::info(); + + meas_airspeed_usage(); + exit(0); +} diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk new file mode 100644 index 000000000..fed4078b6 --- /dev/null +++ b/src/drivers/meas_airspeed/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Makefile to build the MEAS Spec airspeed sensor driver. +# + +MODULE_COMMAND = meas_airspeed +MODULE_STACKSIZE = 2048 + +SRCS = meas_airspeed.cpp diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 220842536..4f7075600 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 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 @@ -35,6 +35,9 @@ * @file mpu6000.cpp * * Driver for the Invensense MPU6000 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey */ #include <nuttx/config.h> @@ -64,8 +67,10 @@ #include <drivers/drv_hrt.h> #include <drivers/device/spi.h> +#include <drivers/device/ringbuffer.h> #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -178,21 +183,33 @@ private: struct hrt_call _call; unsigned _call_interval; - struct accel_report _accel_report; + typedef RingBuffer<accel_report> AccelReportBuffer; + AccelReportBuffer *_accel_reports; + struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - struct gyro_report _gyro_report; + typedef RingBuffer<gyro_report> GyroReportBuffer; + GyroReportBuffer *_gyro_reports; + struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; unsigned _reads; + unsigned _sample_rate; perf_counter_t _sample_perf; + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -204,6 +221,13 @@ private: void stop(); /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -215,7 +239,7 @@ private: static void measure_trampoline(void *arg); /** - * Fetch measurements from the sensor and update the report ring. + * Fetch measurements from the sensor and update the report buffers. */ void measure(); @@ -261,12 +285,26 @@ private: uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } /** - * Self test + * Measurement self test * * @return 0 on success, 1 on failure */ int self_test(); + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + /* set low pass filter frequency */ @@ -297,6 +335,7 @@ protected: void parent_poll_notify(); private: MPU6000 *_parent; + }; /** driver 'main' command */ @@ -307,14 +346,23 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), + _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), _reads(0), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) + _sample_rate(1000), + _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _accel_filter_x(1000, 30), + _accel_filter_y(1000, 30), + _accel_filter_z(1000, 30), + _gyro_filter_x(1000, 30), + _gyro_filter_y(1000, 30), + _gyro_filter_z(1000, 30) { // disable debug() calls _debug_enabled = false; @@ -335,8 +383,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - memset(&_accel_report, 0, sizeof(_accel_report)); - memset(&_gyro_report, 0, sizeof(_gyro_report)); memset(&_call, 0, sizeof(_call)); } @@ -348,6 +394,12 @@ MPU6000::~MPU6000() /* delete the gyro subdriver */ delete _gyro; + /* free any existing reports */ + if (_accel_reports != nullptr) + delete _accel_reports; + if (_gyro_reports != nullptr) + delete _gyro_reports; + /* delete the perf counter */ perf_free(_sample_perf); } @@ -356,6 +408,7 @@ int MPU6000::init() { int ret; + int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -366,9 +419,58 @@ MPU6000::init() return ret; } - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); + /* allocate basic report buffers */ + _accel_reports = new AccelReportBuffer(2); + if (_accel_reports == nullptr) + goto out; + + _gyro_reports = new GyroReportBuffer(2); + if (_gyro_reports == nullptr) + goto out; + + reset(); + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + /* do CDev init for the gyro device node, keep it optional */ + gyro_ret = _gyro->init(); + + /* fetch an initial set of measurements for advertisement */ + measure(); + + if (gyro_ret != OK) { + _gyro_topic = -1; + } else { + gyro_report gr; + _gyro_reports->get(gr); + + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + +out: + return ret; +} + +void MPU6000::reset() +{ // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); @@ -384,13 +486,13 @@ MPU6000::init() // SAMPLE RATE //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(200); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - _set_dlpf_filter(20); + _set_dlpf_filter(42); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -401,12 +503,6 @@ MPU6000::init() // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; @@ -439,12 +535,6 @@ MPU6000::init() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; _accel_range_scale = (9.81f / 4096.0f); _accel_range_m_s2 = 8.0f * 9.81f; @@ -460,14 +550,6 @@ MPU6000::init() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); - /* do CDev init for the gyro device node, keep it optional */ - int gyro_ret = _gyro->init(); - - if (gyro_ret != OK) { - _gyro_topic = -1; - } - - return ret; } int @@ -509,6 +591,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) if(div>200) div=200; if(div<1) div=1; write_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; } /* @@ -545,21 +628,33 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { - int ret = 0; + unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (buflen < sizeof(_accel_report)) + if (count < 1) return -ENOSPC; - /* if automatic measurement is not enabled */ - if (_call_interval == 0) + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); measure(); + } - /* copy out the latest reports */ - memcpy(buffer, &_accel_report, sizeof(_accel_report)); - ret = sizeof(_accel_report); + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) + return -EAGAIN; + + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast<accel_report *>(buffer); + int transferred = 0; + while (count--) { + if (!_accel_reports->get(*arp++)) + break; + transferred++; + } - return ret; + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); } int @@ -573,24 +668,86 @@ MPU6000::self_test() return (_reads > 0) ? 0 : 1; } +int +MPU6000::accel_self_test() +{ + if (self_test()) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +MPU6000::gyro_self_test() +{ + if (self_test()) + return 1; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + return 1; + + if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + return 1; + + return 0; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { - int ret = 0; + unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (buflen < sizeof(_gyro_report)) + if (count < 1) return -ENOSPC; - /* if automatic measurement is not enabled */ - if (_call_interval == 0) + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); measure(); + } - /* copy out the latest report */ - memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); - ret = sizeof(_gyro_report); + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) + return -EAGAIN; + + /* copy reports out of our buffer to the caller */ + gyro_report *arp = reinterpret_cast<gyro_report *>(buffer); + int transferred = 0; + while (count--) { + if (!_gyro_reports->get(*arp++)) + break; + transferred++; + } - return ret; + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); } int @@ -598,6 +755,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCRESET: + reset(); + return OK; + case SENSORIOCSPOLLRATE: { switch (arg) { @@ -617,8 +778,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* XXX 500Hz is just a wild guess */ - return ioctl(filp, SENSORIOCSPOLLRATE, 500); + /* set to same as sample rate per default */ + return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); /* adjust to a legal polling interval in Hz */ default: { @@ -632,6 +793,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; @@ -651,23 +825,51 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; - case SENSORIOCSQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + AccelReportBuffer *buf = new AccelReportBuffer(arg); + + if (nullptr == buf) + return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete _accel_reports; + _accel_reports = buf; + start(); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + return _accel_reports->size(); + case ACCELIOCGSAMPLERATE: + return _sample_rate; case ACCELIOCSSAMPLERATE: - case ACCELIOCGSAMPLERATE: - _set_sample_rate(arg); - return OK; + _set_sample_rate(arg); + return OK; - case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + + // XXX decide on relationship of both filters + // i.e. disable the on-chip filter + //_set_dlpf_filter((uint16_t)arg); + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; case ACCELIOCSSCALE: @@ -689,15 +891,16 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - case ACCELIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_rad_s = 8.0f * 9.81f; + // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; + case ACCELIOCGRANGE: + return _accel_range_m_s2; case ACCELIOCSELFTEST: - return self_test(); + return accel_self_test(); default: /* give it to the superclass */ @@ -713,19 +916,51 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: - case SENSORIOCSQUEUEDEPTH: - case SENSORIOCGQUEUEDEPTH: case SENSORIOCRESET: return ioctl(filp, cmd, arg); - case GYROIOCSSAMPLERATE: + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + GyroReportBuffer *buf = new GyroReportBuffer(arg); + + if (nullptr == buf) + return -ENOMEM; + if (buf->size() == 0) { + delete buf; + return -ENOMEM; + } + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete _gyro_reports; + _gyro_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _gyro_reports->size(); + case GYROIOCGSAMPLERATE: - _set_sample_rate(arg); - return OK; + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _set_sample_rate(arg); + return OK; - case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); + return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + // XXX check relation to the internal lowpass + //_set_dlpf_filter((uint16_t)arg); return OK; case GYROIOCSSCALE: @@ -739,15 +974,16 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: - case GYROIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _gyro_range_scale = xx - // _gyro_range_m_s2 = xx + // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: + return _gyro_range_rad_s; case GYROIOCSELFTEST: - return self_test(); + return gyro_self_test(); default: /* give it to the superclass */ @@ -849,6 +1085,10 @@ MPU6000::start() /* make sure we are stopped first */ stop(); + /* discard any stale data in the buffers */ + _accel_reports->flush(); + _gyro_reports->flush(); + /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this); } @@ -862,7 +1102,7 @@ MPU6000::stop() void MPU6000::measure_trampoline(void *arg) { - MPU6000 *dev = (MPU6000 *)arg; + MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg); /* make another measurement */ dev->measure(); @@ -944,9 +1184,15 @@ MPU6000::measure() report.gyro_y = gyro_yt; /* + * Report buffers. + */ + accel_report arb; + gyro_report grb; + + /* * Adjust and scale results to m/s^2. */ - _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + grb.timestamp = arb.timestamp = hrt_absolute_time(); /* @@ -967,40 +1213,53 @@ MPU6000::measure() /* NOTE: Axes have been swapped to match the board a few lines above. */ - _accel_report.x_raw = report.accel_x; - _accel_report.y_raw = report.accel_y; - _accel_report.z_raw = report.accel_z; + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + 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); + + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + arb.temperature_raw = report.temp; + arb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - _accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - _accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - _accel_report.scaling = _accel_range_scale; - _accel_report.range_m_s2 = _accel_range_m_s2; + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; - _accel_report.temperature_raw = report.temp; - _accel_report.temperature = (report.temp) / 361.0f + 35.0f; + 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); - _gyro_report.x_raw = report.gyro_x; - _gyro_report.y_raw = report.gyro_y; - _gyro_report.z_raw = report.gyro_z; + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; - _gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - _gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - _gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - _gyro_report.scaling = _gyro_range_scale; - _gyro_report.range_rad_s = _gyro_range_rad_s; + grb.temperature_raw = report.temp; + grb.temperature = (report.temp) / 361.0f + 35.0f; - _gyro_report.temperature_raw = report.temp; - _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; + _accel_reports->put(arb); + _gyro_reports->put(grb); /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); } /* stop measuring */ @@ -1063,7 +1322,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU); @@ -1102,21 +1362,19 @@ fail: void test() { - int fd = -1; - int fd_gyro = -1; - struct accel_report a_report; - struct gyro_report g_report; + accel_report a_report; + gyro_report g_report; ssize_t sz; /* get the driver */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", ACCEL_DEVICE_PATH); /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) err(1, "%s open failed", GYRO_DEVICE_PATH); @@ -1128,8 +1386,10 @@ test() /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); - if (sz != sizeof(a_report)) + if (sz != sizeof(a_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(a_report)); err(1, "immediate acc read failed"); + } warnx("single read"); warnx("time: %lld", a_report.timestamp); @@ -1145,8 +1405,10 @@ test() /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); - if (sz != sizeof(g_report)) + if (sz != sizeof(g_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(g_report)); err(1, "immediate gyro read failed"); + } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 59ab5936e..c13b65d5a 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -969,7 +969,8 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + /* if already started, the still command succeeded */ + errx(0, "already started"); /* create the driver */ g_dev = new MS5611(MS5611_BUS); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5147ac500..a98b527b1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -166,7 +166,7 @@ PX4FMU *g_fmu; } // namespace PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), + CDev("fmuservo", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), _pwm_alt_rate(50), diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe..fed83ea1a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,21 +89,61 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +/** + * The PX4IO class. + * + * Encapsulates PX4FMU to PX4IO communications modeled as file operations. + */ class PX4IO : public device::I2C { public: + /** + * Constructor. + * + * Initialize all class variables. + */ PX4IO(); + /** + * Destructor. + * + * Wait for worker thread to terminate. + */ virtual ~PX4IO(); + /** + * Initialize the PX4IO class. + * + * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + */ virtual int init(); + /** + * IO Control handler. + * + * Handle all IOCTL calls to the PX4IO file descriptor. + * + * @param[in] filp file handle (not used). This function is always called directly through object reference + * @param[in] cmd the IOCTL command + * @param[in] the IOCTL command parameter (optional) + */ virtual int ioctl(file *filp, int cmd, unsigned long arg); + + /** + * write handler. + * + * Handle writes to the PX4IO file descriptor. + * + * @param[in] filp file handle (not used). This function is always called directly through object reference + * @param[in] buffer pointer to the data buffer to be written + * @param[in] len size in bytes to be written + * @return number of bytes written + */ virtual ssize_t write(file *filp, const char *buffer, size_t len); /** * Set the update rate for actuator outputs from FMU to IO. * - * @param rate The rate in Hz actuator outpus are sent to IO. + * @param[in] rate The rate in Hz actuator output are sent to IO. * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -111,66 +151,90 @@ public: /** * Set the battery current scaling and bias * - * @param amp_per_volt - * @param amp_bias + * @param[in] amp_per_volt + * @param[in] amp_bias */ void set_battery_current_scaling(float amp_per_volt, float amp_bias); /** * Push failsafe values to IO. * - * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) - * @param len Number of channels, could up to 8 + * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param[in] len Number of channels, could up to 8 */ int set_failsafe_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO - */ + * Print IO status. + * + * Print all relevant IO status information + */ void print_status(); + /** + * Set the DSM VCC is controlled by relay one flag + * + * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled + */ + inline void set_dsm_vcc_ctl(bool enable) + { + _dsm_vcc_ctl = enable; + }; + + /** + * Get the DSM VCC is controlled by relay one flag + * + * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled + */ + inline bool get_dsm_vcc_ctl() + { + return _dsm_vcc_ctl; + }; + private: // XXX - unsigned _max_actuators; - unsigned _max_controls; - unsigned _max_rc_input; - unsigned _max_relays; - unsigned _max_transfer; + unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO + unsigned _max_controls; ///<Maximum # of controls supported by PX4IO + unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO + unsigned _max_relays; ///<Maximum relays supported by PX4IO + unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO - unsigned _update_interval; ///< subscription interval limiting send rate + unsigned _update_interval; ///<Subscription interval limiting send rate - volatile int _task; ///< worker task - volatile bool _task_should_exit; + volatile int _task; ///<worker task id + volatile bool _task_should_exit; ///<worker terminate flag - int _mavlink_fd; + int _mavlink_fd; ///<mavlink file descriptor - perf_counter_t _perf_update; + perf_counter_t _perf_update; ///<local performance counter /* cached IO state */ - uint16_t _status; - uint16_t _alarms; + uint16_t _status; ///<Various IO status flags + uint16_t _alarms; ///<Various IO alarms /* subscribed topics */ - int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status - int _t_param; ///< parameter update topic + int _t_actuators; ///<actuator controls topic + int _t_armed; ///<system armed control topic + int _t_vstatus; ///<system / vehicle status + int _t_param; ///<parameter update topic /* advertised topics */ - orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic - orb_advert_t _to_outputs; ///< mixed servo outputs topic - orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_input_rc; ///<rc inputs from IO topic + orb_advert_t _to_actuators_effective; ///<effective actuator controls topic + orb_advert_t _to_outputs; ///<mixed servo outputs topic + orb_advert_t _to_battery; ///<battery status / voltage topic + + actuator_outputs_s _outputs; ///<mixed outputs + actuator_controls_effective_s _controls_effective; ///<effective controls - actuator_outputs_s _outputs; ///< mixed outputs - actuator_controls_effective_s _controls_effective; ///< effective controls + bool _primary_pwm_device; ///<true if we are the default PWM output - bool _primary_pwm_device; ///< true if we are the default PWM output + float _battery_amp_per_volt; ///<current sensor amps/volt + float _battery_amp_bias; ///<current sensor bias + float _battery_mamphour_total;///<amp hours consumed so far + uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp - float _battery_amp_per_volt; - float _battery_amp_bias; - float _battery_mamphour_total; - uint64_t _battery_last_timestamp; + bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power /** * Trampoline to the worker task @@ -313,7 +377,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -338,7 +402,8 @@ PX4IO::PX4IO() : _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _dsm_vcc_ctl(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -553,9 +618,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -655,6 +722,25 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; + int32_t dsm_bind_val; + param_t dsm_bind_param; + + // See if bind parameter has been set, and reset it to 0 + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) { + if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + dsm_bind_val = 0; + param_set(dsm_bind_param, &dsm_bind_val); + } /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -700,8 +786,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1271,13 +1355,14 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), @@ -1372,7 +1457,8 @@ PX4IO::print_status() } int -PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) +/* Make it obvious that file * isn't used here */ { int ret = OK; @@ -1424,6 +1510,21 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case DSM_BIND_START: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(50000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + usleep(50000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + break; + + case DSM_BIND_POWER_UP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { /* TODO: we could go lower for e.g. TurboPWM */ @@ -1466,18 +1567,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case GPIO_RESET: - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); + case GPIO_RESET: { + uint32_t bits = (1 << _max_relays) - 1; + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_RELAY1; + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; + } case GPIO_SET: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: @@ -1553,7 +1667,8 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } ssize_t -PX4IO::write(file *filp, const char *buffer, size_t len) +PX4IO::write(file * /*filp*/, const char *buffer, size_t len) +/* Make it obvious that file * isn't used here */ { unsigned count = len / 2; @@ -1614,7 +1729,44 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + int dsm_vcc_ctl; + + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { + if (dsm_vcc_ctl) { + g_dev->set_dsm_vcc_ctl(true); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); + } + } + exit(0); +} + +void +bind(int argc, char *argv[]) +{ + int pulses; + + if (g_dev == nullptr) + errx(1, "px4io must be started first"); + + if (!g_dev->get_dsm_vcc_ctl()) + errx(1, "DSM bind feature not enabled"); + + if (argc < 3) + errx(0, "needs argument, use dsm2 or dsmx"); + + if (!strcmp(argv[2], "dsm2")) + pulses = 3; + else if (!strcmp(argv[2], "dsmx")) + pulses = 7; + else + errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + + warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); + + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); + exit(0); + } void @@ -1626,7 +1778,7 @@ test(void) int direction = 1; int ret; - fd = open("/dev/px4io", O_WRONLY); + fd = open(PX4IO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); @@ -1800,7 +1952,7 @@ px4io_main(int argc, char *argv[]) * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1); } else { errx(1, "not loaded"); } @@ -1844,7 +1996,7 @@ px4io_main(int argc, char *argv[]) /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); + int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); @@ -1918,6 +2070,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); + if (!strcmp(argv[1], "bind")) + bind(argc, argv); + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'"); } diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 7ef3db970..83a1a1abb 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -70,7 +70,7 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef CONFIG_HRT_TIMER +#ifdef HRT_TIMER /* HRT configuration */ #if HRT_TIMER == 1 @@ -155,7 +155,7 @@ # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif #else -# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y +# error HRT_TIMER must be a value between 1 and 11 #endif /* @@ -275,7 +275,7 @@ static void hrt_call_invoke(void); /* * Specific registers and bits used by PPM sub-functions */ -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * @@ -326,7 +326,7 @@ static void hrt_call_invoke(void); # define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ # define CCER_PPM_FLIP GTIM_CCER_CC4P # else -# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set +# error HRT_PPM_CHANNEL must be a value between 1 and 4 # endif /* @@ -377,7 +377,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCMR1_PPM 0 # define CCMR2_PPM 0 # define CCER_PPM 0 -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Initialise the timer we are going to use. @@ -424,7 +424,7 @@ hrt_tim_init(void) up_enable_irq(HRT_TIMER_VECTOR); } -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * Handle the PPM decoder state machine. */ @@ -526,7 +526,7 @@ error: ppm_decoded_channels = 0; } -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Handle the compare interupt by calling the callout dispatcher @@ -546,7 +546,7 @@ hrt_tim_isr(int irq, void *context) /* ack the interrupts we just read */ rSR = ~status; -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* was this a PPM edge? */ if (status & (SR_INT_PPM | SR_OVF_PPM)) { @@ -686,7 +686,7 @@ hrt_init(void) sq_init(&callout_queue); hrt_tim_init(); -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* configure the PPM input pin */ stm32_configgpio(GPIO_PPM_IN); #endif @@ -907,4 +907,4 @@ hrt_latency_update(void) } -#endif /* CONFIG_HRT_TIMER */ +#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 7b060412c..dbb45a138 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -88,6 +88,7 @@ #define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) static void pwm_timer_init(unsigned timer); static void pwm_timer_set_rate(unsigned timer, unsigned rate); @@ -110,6 +111,11 @@ pwm_timer_init(unsigned timer) rCCER(timer) = 0; rDCR(timer) = 0; + if ((pwm_timers[timer].base == STM32_TIM1_BASE) || (pwm_timers[timer].base == STM32_TIM8_BASE)) { + /* master output enable = on */ + rBDTR(timer) = ATIM_BDTR_MOE; + } + /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 167ef30a8..2284be84d 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -117,10 +117,6 @@ #include <systemlib/err.h> -#ifndef CONFIG_HRT_TIMER -# error This driver requires CONFIG_HRT_TIMER -#endif - /* Tone alarm configuration */ #if TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 97d7fdd75..191d20f30 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -661,10 +661,10 @@ int KalmanNav::correctPos() Vector y(6); y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; - y(4) = double(_gps.alt) / 1.0e3 - alt; - y(5) = double(_sensors.baro_alt_meter) - alt; + y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; + y(4) = _gps.alt / 1.0e3f - alt; + y(5) = _sensors.baro_alt_meter - alt; // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -698,7 +698,7 @@ int KalmanNav::correctPos() vD += xCorrect(VD); lat += double(xCorrect(LAT)); lon += double(xCorrect(LON)); - alt += double(xCorrect(ALT)); + alt += xCorrect(ALT); // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -710,7 +710,7 @@ int KalmanNav::correctPos() static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", double(y(0) / sqrtf(RPos(0, 0))), double(y(1) / sqrtf(RPos(1, 1))), double(y(2) / sqrtf(RPos(2, 2))), diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 49d0d157d..f01ee0355 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include <mathlib/mathlib.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -#include <controllib/block/UOrbSubscription.hpp> -#include <controllib/block/UOrbPublication.hpp> +#include <controllib/uorb/UOrbSubscription.hpp> +#include <controllib/uorb/UOrbPublication.hpp> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 4befdc879..372b2d162 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -121,12 +121,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("is running\n"); + exit(0); } else { warnx("not started\n"); + exit(1); } - exit(0); } usage("unrecognized command"); 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 d8b40ac3b..9e533ccdf 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_ekf app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_ekf app not started\n"); + warnx("not started"); + exit(1); } exit(0); @@ -222,8 +224,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -234,7 +236,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - int loopcounter = 0; int printcounter = 0; @@ -382,7 +383,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { + if (!const_initialized && dt < 0.05f && dt > 0.001f) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -422,7 +423,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp - last_data > 12000) + printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39..107c2dfb1 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -139,10 +139,12 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tattitude_estimator_so3_comp app is running\n"); + warnx("running"); + exit(0); } else { - printf("\tattitude_estimator_so3_comp app not started\n"); + warnx("not started"); + exit(1); } exit(0); diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 48a36ac26..fbb73d997 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -226,6 +226,12 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float if (orient < 0) return ERROR; + if (data_collected[orient]) { + sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + continue; + } + sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); @@ -268,8 +274,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ float ema_len = 0.2f; - /* set "still" threshold to 0.1 m/s^2 */ - float still_thr2 = pow(0.1f, 2); + /* set "still" threshold to 0.25 m/s^2 */ + float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ @@ -283,6 +289,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { hrt_abstime t = t_start; hrt_abstime t_prev = t_start; hrt_abstime t_still = 0; + + unsigned poll_errcount = 0; + while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); @@ -327,12 +336,14 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "ERROR: poll failure"); - return -3; + poll_errcount++; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); return -1; } } @@ -375,6 +386,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + int errcount = 0; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { @@ -384,8 +397,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { - return ERROR; + errcount++; + continue; } + + if (errcount > samples_num / 10) + return ERROR; } for (int i = 0; i < 3; i++) { diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 67f053e22..e9d1f3954 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -282,7 +282,7 @@ void tune_error(void) void do_rc_calibration(int status_pub, struct vehicle_status_s *status) { - if (current_status.offboard_control_signal_lost) { + if (current_status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); return; } @@ -587,6 +587,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(fd); + int errcount = 0; + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -602,8 +604,12 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + errcount++; + } + + if (errcount > 1000) { + /* any persisting poll error is a reason to abort */ + mavlink_log_info(mavlink_fd, "permanent gyro error, aborted."); return; } } @@ -1389,6 +1395,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -1828,8 +1835,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + /* bottom stick position, set default */ + /* this MUST be mapped to extremal position to switch easy in case of emergency */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { @@ -1837,8 +1845,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + /* center stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + } + + if (current_status.manual_sas_mode != manual_sas_mode) { + /* publish SAS mode changes immediately */ + manual_sas_mode = current_status.manual_sas_mode; + state_changed = true; } /* @@ -1849,8 +1863,10 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + current_status.flag_control_manual_enabled && + current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + sp_man.yaw < -STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1862,7 +1878,10 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { + if (current_status.flag_control_manual_enabled && + current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + sp_man.yaw > STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 5994d2315..b964d40a3 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include "../uorb/UOrbSubscription.hpp" +#include "../uorb/UOrbPublication.hpp" namespace control { diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7a785d12e..fefe99702 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -42,6 +42,7 @@ #include <assert.h> #include <time.h> #include <stdlib.h> +#include <math.h> #include <mathlib/math/test/test.hpp> #include "block/Block.hpp" diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index 13d1069c7..d815a8feb 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,7 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - block/UOrbPublication.cpp \ - block/UOrbSubscription.cpp \ - blocks.cpp \ - fixedwing.cpp + uorb/UOrbPublication.cpp \ + uorb/UOrbSubscription.cpp \ + uorb/blocks.cpp \ + blocks.cpp diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp index f69b39d90..f69b39d90 100644 --- a/src/modules/controllib/block/UOrbPublication.cpp +++ b/src/modules/controllib/uorb/UOrbPublication.cpp diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp index 0a8ae2ff7..6f1f3fc1c 100644 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ b/src/modules/controllib/uorb/UOrbPublication.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp index 022cadd24..022cadd24 100644 --- a/src/modules/controllib/block/UOrbSubscription.cpp +++ b/src/modules/controllib/uorb/UOrbSubscription.cpp diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp index 22cc2e114..d337d89a8 100644 --- a/src/modules/controllib/block/UOrbSubscription.hpp +++ b/src/modules/controllib/uorb/UOrbSubscription.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp new file mode 100644 index 000000000..448a42a99 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 uorb_blocks.cpp + * + * uorb block library code + */ + +#include "blocks.hpp" + +namespace control +{ + +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +} // namespace control + diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp new file mode 100644 index 000000000..9c0720aa5 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 uorb_blocks.h + * + * uorb block library code + */ + +#pragma once + +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <drivers/drv_hrt.h> +#include <poll.h> + +extern "C" { +#include <systemlib/geo/geo.h> +} + +#include "../blocks.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +/** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription<vehicle_attitude_s> _att; + UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; + UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; + UOrbSubscription<vehicle_global_position_s> _pos; + UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; + UOrbSubscription<manual_control_setpoint_s> _manual; + UOrbSubscription<vehicle_status_s> _status; + UOrbSubscription<parameter_update_s> _param_update; + // publications + UOrbPublication<actuator_controls_s> _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +} // namespace control + diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 77b2ac806..f655a13bd 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,61 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _yawDamper.update(rCmd, r, outputScale); } -BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _xtYawLimit(this, "XT2YAW"), - _xt2Yaw(this, "XT2YAW"), - _psiCmd(0) -{ -} - -BlockWaypointGuidance::~BlockWaypointGuidance() {}; - -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) -{ - - // heading to waypoint - float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - // cross track - struct crosstrack_error_s xtrackError; - get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - _psiCmd = _wrap_2pi(psiTrack - - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); -} - -BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) -{ -} - -BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index e4028c40d..3876e4630 100644 --- a/src/modules/controllib/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -39,31 +39,8 @@ #pragma once -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> - -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <math.h> - -#include <drivers/drv_hrt.h> -#include <poll.h> - -#include "blocks.hpp" -#include "block/UOrbSubscription.hpp" -#include "block/UOrbPublication.hpp" - -extern "C" { -#include <systemlib/geo/geo.h> -} +#include <controllib/blocks.hpp> +#include <controllib/uorb/blocks.hpp> namespace control { @@ -251,47 +228,6 @@ public: */ /** - * Waypoint Guidance block - */ -class __EXPORT BlockWaypointGuidance : public SuperBlock -{ -private: - BlockLimitSym _xtYawLimit; - BlockP _xt2Yaw; - float _psiCmd; -public: - BlockWaypointGuidance(SuperBlock *parent, const char *name); - virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); - float getPsiCmd() { return _psiCmd; } -}; - -/** - * UorbEnabledAutopilot - */ -class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock -{ -protected: - // subscriptions - UOrbSubscription<vehicle_attitude_s> _att; - UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; - UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; - UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; - UOrbSubscription<manual_control_setpoint_s> _manual; - UOrbSubscription<vehicle_status_s> _status; - UOrbSubscription<parameter_update_s> _param_update; - // publications - UOrbPublication<actuator_controls_s> _actuators; -public: - BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); - virtual ~BlockUorbEnabledAutopilot(); -}; - -/** * Multi-mode Autopilot */ class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..f4ea05088 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -45,12 +45,13 @@ #include <stdlib.h> #include <string.h> #include <systemlib/systemlib.h> -#include <controllib/fixedwing.hpp> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <math.h> +#include "fixedwing.hpp" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk index ec958d7cb..133728781 100644 --- a/src/modules/fixedwing_backside/module.mk +++ b/src/modules/fixedwing_backside/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fixedwing_backside SRCS = fixedwing_backside_main.cpp \ + fixedwing.cpp \ params.c diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 8b4c0cb30..1aef739c7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,11 +53,7 @@ #include <uORB/topics/vehicle_status.h> #include <poll.h> #include <drivers/drv_gpio.h> - -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#include <modules/px4iofirmware/protocol.h> struct gpio_led_s { struct work_s work; @@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; if (priv->use_io) { - gpio_dev = "/dev/px4io"; - + gpio_dev = PX4IO_DEVICE_PATH; } else { - gpio_dev = "/dev/px4fmu"; + gpio_dev = PX4FMU_DEVICE_PATH; } /* open GPIO device */ @@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ + /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ @@ -263,7 +259,6 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); - } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp new file mode 100644 index 000000000..efb17225d --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp @@ -0,0 +1,77 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.cpp +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall <LeonardTHall@gmail.com> + +#include "LowPassFilter2p.hpp" +#include "math.h" + +namespace math +{ + +void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) +{ + _cutoff_freq = cutoff_freq; + float fr = sample_freq/_cutoff_freq; + float ohm = tanf(M_PI_F/fr); + float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; + _b0 = ohm*ohm/c; + _b1 = 2.0f*_b0; + _b2 = _b0; + _a1 = 2.0f*(ohm*ohm-1.0f)/c; + _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; +} + +float LowPassFilter2p::apply(float 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)) { + // don't allow bad values to propogate via the filter + delay_element_0 = sample; + } + float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + + // return the value. Should be no need to check limits + return output; +} + +} // namespace math + diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp new file mode 100644 index 000000000..208ec98d4 --- /dev/null +++ b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * 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 LowPassFilter.h +/// @brief A class to implement a second order low pass filter +/// Author: Leonard Hall <LeonardTHall@gmail.com> +/// Adapted for PX4 by Andrew Tridgell + +#pragma once + +namespace math +{ +class __EXPORT LowPassFilter2p +{ +public: + // constructor + LowPassFilter2p(float sample_freq, float cutoff_freq) { + // set initial parameters + set_cutoff_frequency(sample_freq, cutoff_freq); + _delay_element_1 = _delay_element_2 = 0; + } + + // change parameters + void set_cutoff_frequency(float sample_freq, float cutoff_freq); + + // apply - Add a new raw value to the filter + // and retrieve the filtered result + float apply(float sample); + + // return the cutoff frequency + float get_cutoff_freq(void) const { + return _cutoff_freq; + } + +private: + float _cutoff_freq; + float _a1; + float _a2; + float _b0; + float _b1; + float _b2; + float _delay_element_1; // buffered sample -1 + float _delay_element_2; // buffered sample -2 +}; + +} // namespace math diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/mathlib/math/filter/module.mk new file mode 100644 index 000000000..fe92c8c70 --- /dev/null +++ b/src/modules/mathlib/math/filter/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# filter library +# +SRCS = LowPassFilter2p.cpp + +# +# In order to include .config we first have to save off the +# current makefile name, since app.mk needs it. +# +APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) +-include $(TOPDIR)/.config diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7c1c4b175..919d01561 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -420,12 +420,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d\n", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -433,37 +433,35 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf int termios_state; *is_usb = false; - /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } + } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; } return uart; @@ -751,8 +749,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(uart, TCSANOW, &uart_config_original); thread_running = false; diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c index d9416a08b..192195856 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/mavlink/mavlink_log.c @@ -41,16 +41,20 @@ #include <string.h> #include <stdlib.h> +#include <stdio.h> #include <stdarg.h> #include <mavlink/mavlink_log.h> +static FILE* text_recorder_fd = NULL; + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); + text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) @@ -82,6 +86,13 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; + + if (text_recorder_fd) { + fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); + fputc("\n", text_recorder_fd); + fsync(text_recorder_fd); + } + return 0; } else { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 405046750..eea928a17 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -373,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..fbd82a4c6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -95,9 +95,16 @@ controls_tick() { */ perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + else + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -293,6 +300,8 @@ controls_tick() { } else { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..745cdfa40 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -40,6 +40,7 @@ */ #include <nuttx/config.h> +#include <nuttx/arch.h> #include <fcntl.h> #include <unistd.h> @@ -47,121 +48,44 @@ #include <drivers/drv_hrt.h> -#define DEBUG - #include "px4io.h" -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -static int dsm_fd = -1; - -static hrt_abstime last_rx_time; -static hrt_abstime last_frame_time; - -static uint8_t frame[DSM_FRAME_SIZE]; - -static unsigned partial_frame_count; -static unsigned channel_shift; - -unsigned dsm_frame_drops; - -static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); -static void dsm_guess_format(bool reset); -static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); - -int -dsm_init(const char *device) -{ - if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY | O_NONBLOCK); - - if (dsm_fd >= 0) { - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(dsm_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(dsm_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - /* reset the format detector */ - dsm_guess_format(true); - - debug("DSM: ready"); - - } else { - debug("DSM: open failed"); - } - - return dsm_fd; -} - -bool -dsm_input(uint16_t *values, uint16_t *num_values) -{ - ssize_t ret; - hrt_abstime now; +#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ +#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ - /* - * The DSM* protocol doesn't provide any explicit framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 11ms; with 16 bytes at 115200bps - * frame transmission time is ~1.4ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); - - if ((now - last_rx_time) > 5000) { - if (partial_frame_count > 0) { - dsm_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return dsm_decode(now, values, num_values); -} +static int dsm_fd = -1; /**<File handle to the DSM UART*/ +static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/ +static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/ +static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/ +static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/ +static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/ +static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/ +/** + * Attempt to decode a single channel raw channel datum + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * + * Upon receiving a full dsm frame we attempt to decode it + * + * @param[in] raw 16 bit raw channel value from dsm frame + * @param[in] shift position of channel number in raw data + * @param[out] channel pointer to returned channel number + * @param[out] value pointer to returned channel value + * @return true=raw value successfully decoded + */ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { @@ -179,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va return true; } +/** + * Attempt to guess if receiving 10 or 11 bit channel values + * + * @param[in] reset true=reset the 10/11 bit state to unknown + */ static void dsm_guess_format(bool reset) { @@ -191,14 +120,14 @@ dsm_guess_format(bool reset) cs10 = 0; cs11 = 0; samples = 0; - channel_shift = 0; + dsm_channel_shift = 0; return; } - /* scan the channels in the current frame in both 10- and 11-bit mode */ + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; @@ -209,16 +138,16 @@ dsm_guess_format(bool reset) if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) cs11 |= (1 << channel); - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - /* wait until we have seen plenty of frames - 2 should normally be enough */ + /* wait until we have seen plenty of frames - 5 should normally be enough */ if (samples++ < 5) return; /* * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. + * decoding in 10 or 11-bit mode has yielded anything we recognize. * * XXX Note that due to what seem to be bugs in the DSM2 high-resolution * stream, we may want to sniff for longer in some cases when we think we @@ -248,13 +177,13 @@ dsm_guess_format(bool reset) } if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; + dsm_channel_shift = 11; debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; + dsm_channel_shift = 10; debug("DSM: 10-bit format"); return; } @@ -264,27 +193,131 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + + if (dsm_fd >= 0) { + + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + dsm_partial_frame_count = 0; + dsm_last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + + } else { + + debug("DSM: open failed"); + + } + + return dsm_fd; +} + +/** + * Handle DSM satellite receiver bind mode handler + * + * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart + * @param[in] pulses Number of pulses for dsm_bind_send_pulses command + */ +void +dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = + GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + + case dsm_bind_power_down: + + /*power down DSM satellite*/ + POWER_RELAY1(0); + break; + + case dsm_bind_power_up: + + /*power up DSM satellite*/ + POWER_RELAY1(1); + dsm_guess_format(true); + break; + + case dsm_bind_set_rx_out: + + /*Set UART RX pin to active output mode*/ + stm32_configgpio(usart1RxAsOutp); + break; + + case dsm_bind_send_pulses: + + /*Pulse RX pin a number of times*/ + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(25); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(25); + } + break; + + case dsm_bind_reinit_uart: + + /*Restore USART RX pin to RS232 receive mode*/ + stm32_configgpio(GPIO_USART1_RX); + break; + + } +} + +/** + * Decode the entire dsm frame (all contained channels) + * + * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess. + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=DSM frame successfully decoded, false=no update + */ static bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { - /* - debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], - frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); + debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], + dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); */ /* * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) dsm_guess_format(true); - /* we have received something we think is a frame */ - last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; - /* if we don't know the frame format, update the guessing state machine */ - if (channel_shift == 0) { + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (dsm_channel_shift == 0) { dsm_guess_format(false); return false; } @@ -296,17 +329,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Each channel is a 16-bit unsigned value containing either a 10- * or 11-bit channel value and a 4-bit channel number, shifted * either 10 or 11 bits. The MSB may also be set to indicate the - * second frame in variants of the protocol where more than + * second dsm_frame in variants of the protocol where more than * seven channels are being transmitted. */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) continue; /* ignore channels out of range */ @@ -318,7 +351,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) *num_values = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (channel_shift == 11) + if (dsm_channel_shift == 11) value /= 2; value += 998; @@ -349,8 +382,78 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + if (dsm_channel_shift == 11) + *num_values |= 0x8000; + /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ return true; } + +/** + * Called periodically to check for input data from the DSM UART + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * Upon receiving a full dsm frame we attempt to decode it. + * + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=decoded raw channel values updated, false=no update + */ +bool +dsm_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + */ + now = hrt_absolute_time(); + + if ((now - dsm_last_rx_time) > 5000) { + if (dsm_partial_frame_count > 0) { + dsm_frame_drops++; + dsm_partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current dsm frame. + */ + ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + dsm_last_rx_time = now; + + /* + * Add bytes to the current dsm frame + */ + dsm_partial_frame_count += ret; + + /* + * If we don't have a full dsm frame, return + */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a dsm frame. Go ahead and + * decode it. + */ + dsm_partial_frame_count = 0; + return dsm_decode(now, values, num_values); +} diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd..88d8cc87c 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ @@ -156,7 +157,22 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ + +/* px4io relay bit definitions */ +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..57cffcc23 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values); @@ -191,9 +192,9 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); extern volatile uint8_t debug_level; /* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); +extern void isr_debug(uint8_t level, const char *fmt, ...); #ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); +void i2c_dump(void); +void i2c_reset(void); #endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd3..a922362b6 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & (1 << 0) ? 1 : 0); - POWER_RELAY2(value & (1 << 1) ? 1 : 0); - POWER_ACC1(value & (1 << 2) ? 1 : 0); - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); + POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); + POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); + POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); break; case PX4IO_P_SETUP_SET_DEBUG: @@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 7); + break; + default: return -1; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3e6b20472..ba7cdd91c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/optical_flow.h> @@ -603,6 +604,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(150); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -615,7 +625,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 18; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -637,6 +647,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -660,6 +671,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; + int global_pos_sp_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -689,6 +701,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; + struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; } body; } log_msg = { @@ -775,6 +788,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL POSITION SETPOINT--- */ + subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + fds[fdsc_count].fd = subs.global_pos_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; @@ -1077,6 +1096,25 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPOS); } + /* --- GLOBAL POSITION SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; + log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; + log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; + log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; + log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; + log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; + log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; + log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; + log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; + log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; + log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } + /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); @@ -1227,7 +1265,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abc882d23..934e4dec8 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,15 +149,15 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - unsigned char state; - unsigned char flight_mode; - unsigned char manual_control_mode; - unsigned char manual_sas_mode; - unsigned char armed; + uint8_t state; + uint8_t flight_mode; + uint8_t manual_control_mode; + uint8_t manual_sas_mode; + uint8_t armed; float battery_voltage; float battery_current; float battery_remaining; - unsigned char battery_warning; + uint8_t battery_warning; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -210,13 +210,29 @@ struct log_GPOS_s { float vel_d; }; +/* --- GPSP - GLOBAL POSITION SETPOINT --- */ +#define LOG_GPSP_MSG 17 +struct log_GPSP_s { + uint8_t altitude_is_relative; + int32_t lat; + int32_t lon; + float altitude; + float yaw; + float loiter_radius; + int8_t loiter_direction; + uint8_t nav_cmd; + float param1; + float param2; + float param3; + float param4; +}; + /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 64 +#define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; uint8_t esc_count; uint8_t esc_connectiontype; - uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; @@ -227,6 +243,7 @@ struct log_ESC_s { float esc_setpoint; uint16_t esc_setpoint_raw; }; + #pragma pack(pop) /* construct list of all message formats */ @@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), }; diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 000000000..b1dc39445 --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,57 @@ +#include "BlockSegwayController.hpp" + +void BlockSegwayController::update() { + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + } + + // compute speed command + float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } + } + + // update all publications + updatePublications(); + +} + diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp new file mode 100644 index 000000000..4a01f785c --- /dev/null +++ b/src/modules/segway/BlockSegwayController.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include <controllib/uorb/blocks.hpp> + +using namespace control; + +class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +public: + BlockSegwayController() : + BlockUorbEnabledAutopilot(NULL,"SEG"), + th2v(this, "TH2V"), + q2v(this, "Q2V"), + _attPoll(), + _timeStamp(0) + { + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; + } + void update(); +private: + enum {CH_LEFT, CH_RIGHT}; + BlockPI th2v; + BlockP q2v; + struct pollfd _attPoll; + uint64_t _timeStamp; +}; + diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk new file mode 100644 index 000000000..d5da85601 --- /dev/null +++ b/src/modules/segway/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 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. +# +############################################################################ + +# +# segway controller +# + +MODULE_COMMAND = segway + +SRCS = segway_main.cpp \ + BlockSegwayController.cpp \ + params.c diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c new file mode 100644 index 000000000..d72923717 --- /dev/null +++ b/src/modules/segway/params.c @@ -0,0 +1,8 @@ +#include <systemlib/param/param.h> + +// 16 is max name length +PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage + diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp new file mode 100644 index 000000000..061fbf9b9 --- /dev/null +++ b/src/modules/segway/segway_main.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert + * + * 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 segway_main.cpp + * @author James Goppert + * + * Segway controller using control library + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <math.h> + +#include "BlockSegwayController.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int segway_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int segway_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * 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(). + */ +int segway_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + + deamon_task = task_spawn_cmd("segway", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + segway_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int segway_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + BlockSegwayController autopilot; + + thread_running = true; + + while (!thread_should_exit) { + autopilot.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index f6f4d60c7..bd431c9eb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -155,6 +155,8 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ +PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1ded14a91..42268b971 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -60,6 +60,7 @@ #include <drivers/drv_baro.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_adc.h> +#include <drivers/drv_airspeed.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -91,8 +92,35 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + #define ADC_BATTERY_VOLTAGE_CHANNEL 10 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + #define ADC_BATTERY_VOLTAGE_CHANNEL 2 + #define ADC_BATTERY_CURRENT_CHANNEL 3 + #define ADC_5V_RAIL_SENSE 4 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif #define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f @@ -139,14 +167,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ -#if CONFIG_HRT_PPM hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ /** * Gather and publish PPM input data. */ void ppm_poll(); -#endif /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -199,7 +225,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; - int diff_pres_offset_pa; + float diff_pres_offset_pa; int rc_type; @@ -230,6 +256,7 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -278,6 +305,7 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + } _parameter_handles; /**< handles for interesting parameters */ @@ -389,13 +417,11 @@ namespace sensors #endif static const int ERROR = -1; -Sensors *g_sensors; +Sensors *g_sensors = nullptr; } Sensors::Sensors() : -#ifdef CONFIG_HRT_PPM _ppm_last_valid(0), -#endif _fd_adc(-1), _last_adc(0), @@ -551,25 +577,11 @@ Sensors::parameters_update() /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { - if (param_get(_parameter_handles.min[i], &(_parameters.min[i])) != OK) { - warnx("Failed getting min for chan %d", i); - } - - if (param_get(_parameter_handles.trim[i], &(_parameters.trim[i])) != OK) { - warnx("Failed getting trim for chan %d", i); - } - - if (param_get(_parameter_handles.max[i], &(_parameters.max[i])) != OK) { - warnx("Failed getting max for chan %d", i); - } - - if (param_get(_parameter_handles.rev[i], &(_parameters.rev[i])) != OK) { - warnx("Failed getting rev for chan %d", i); - } - - if (param_get(_parameter_handles.dz[i], &(_parameters.dz[i])) != OK) { - warnx("Failed getting dead zone for chan %d", i); - } + param_get(_parameter_handles.min[i], &(_parameters.min[i])); + param_get(_parameter_handles.trim[i], &(_parameters.trim[i])); + param_get(_parameter_handles.max[i], &(_parameters.max[i])); + param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); + param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); @@ -659,21 +671,10 @@ Sensors::parameters_update() warnx("Failed getting mode aux 5 index"); } - if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { - warnx("Failed getting rc scaling for roll"); - } - - if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) { - warnx("Failed getting rc scaling for pitch"); - } - - if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { - warnx("Failed getting rc scaling for yaw"); - } - - if (param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)) != OK) { - warnx("Failed getting rc scaling for flaps"); - } + param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); + param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); + param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); + param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -745,11 +746,26 @@ Sensors::accel_init() errx(1, "FATAL: no accelerometer found"); } else { - /* set the accel internal sampling rate up to at leat 500Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, 500); - /* set the driver to poll at 500Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 500); + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + /* set the accel internal sampling rate up to at leat 1000Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 1000); + + /* set the driver to poll at 1000Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 1000); + + #else + + /* set the accel internal sampling rate up to at leat 800Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif warnx("using system accel"); close(fd); @@ -768,11 +784,28 @@ Sensors::gyro_init() errx(1, "FATAL: no gyro found"); } else { - /* set the gyro internal sampling rate up to at leat 500Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 500); - /* set the driver to poll at 500Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 500); + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + /* set the gyro internal sampling rate up to at least 1000Hz */ + if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 1000Hz */ + if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #else + + /* set the gyro internal sampling rate up to at leat 800Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif warnx("using system gyro"); close(fd); @@ -1033,6 +1066,20 @@ Sensors::parameter_update_poll(bool forced) close(fd); + fd = open(AIRSPEED_DEVICE_PATH, 0); + + /* this sensor is optional, abort without error */ + + if (fd > 0) { + struct airspeed_scale airscale = { + _parameters.diff_pres_offset_pa, + 1.0f, + }; + + if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + #if 0 printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); @@ -1123,16 +1170,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } -#if CONFIG_HRT_PPM void Sensors::ppm_poll() { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - bool rc_updated; - orb_check(_rc_sub, &rc_updated); + struct pollfd fds[1]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + /* check non-blocking for new data */ + int poll_ret = poll(fds, 1, 0); - if (rc_updated) { + if (poll_ret > 0) { struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1320,7 +1369,6 @@ Sensors::ppm_poll() } } -#endif void Sensors::task_main_trampoline(int argc, char *argv[]) @@ -1359,6 +1407,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ @@ -1394,7 +1445,7 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ @@ -1433,10 +1484,8 @@ Sensors::task_main() if (_publishing) orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); -#ifdef CONFIG_HRT_PPM /* Look for new r/c input data */ ppm_poll(); -#endif perf_end(_loop_perf); } @@ -1476,7 +1525,7 @@ int sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (sensors::g_sensors != nullptr) - errx(1, "sensors task already running"); + errx(0, "sensors task already running"); sensors::g_sensors = new Sensors; diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 15bb833a9..e01cc4dda 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure) if (differential_pressure > 0) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { - return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } } @@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp return sqrtf((2.0f*(pressure_difference)) / density); } else { - return -sqrtf((2.0f*fabs(pressure_difference)) / density); + return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8d77f14a8..8e9c2bfcf 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -53,14 +53,26 @@ struct hx_stream { - uint8_t buf[HX_STREAM_MAX_FRAME + 4]; - unsigned frame_bytes; - bool escaped; - bool txerror; - + /* RX state */ + uint8_t rx_buf[HX_STREAM_MAX_FRAME + 4]; + unsigned rx_frame_bytes; + bool rx_escaped; + hx_stream_rx_callback rx_callback; + void *rx_callback_arg; + + /* TX state */ int fd; - hx_stream_rx_callback callback; - void *callback_arg; + bool tx_error; + uint8_t *tx_buf; + unsigned tx_resid; + uint32_t tx_crc; + enum { + TX_IDLE = 0, + TX_SEND_START, + TX_SEND_DATA, + TX_SENT_ESCAPE, + TX_SEND_END + } tx_state; perf_counter_t pc_tx_frames; perf_counter_t pc_rx_frames; @@ -81,21 +93,7 @@ static void hx_tx_raw(hx_stream_t stream, uint8_t c) { if (write(stream->fd, &c, 1) != 1) - stream->txerror = true; -} - -static void -hx_tx_byte(hx_stream_t stream, uint8_t c) -{ - switch (c) { - case FBO: - case CEO: - hx_tx_raw(stream, CEO); - c ^= 0x20; - break; - } - - hx_tx_raw(stream, c); + stream->tx_error = true; } static int @@ -105,11 +103,11 @@ hx_rx_frame(hx_stream_t stream) uint8_t b[4]; uint32_t w; } u; - unsigned length = stream->frame_bytes; + unsigned length = stream->rx_frame_bytes; /* reset the stream */ - stream->frame_bytes = 0; - stream->escaped = false; + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; /* not a real frame - too short */ if (length < 4) { @@ -122,11 +120,11 @@ hx_rx_frame(hx_stream_t stream) length -= 4; /* compute expected CRC */ - u.w = crc32(&stream->buf[0], length); + u.w = crc32(&stream->rx_buf[0], length); /* compare computed and actual CRC */ for (unsigned i = 0; i < 4; i++) { - if (u.b[i] != stream->buf[length + i]) { + if (u.b[i] != stream->rx_buf[length + i]) { perf_count(stream->pc_rx_errors); return 0; } @@ -134,7 +132,7 @@ hx_rx_frame(hx_stream_t stream) /* frame is good */ perf_count(stream->pc_rx_frames); - stream->callback(stream->callback_arg, &stream->buf[0], length); + stream->rx_callback(stream->rx_callback_arg, &stream->rx_buf[0], length); return 1; } @@ -150,8 +148,8 @@ hx_stream_init(int fd, if (stream != NULL) { memset(stream, 0, sizeof(struct hx_stream)); stream->fd = fd; - stream->callback = callback; - stream->callback_arg = arg; + stream->rx_callback = callback; + stream->rx_callback_arg = arg; } return stream; @@ -179,49 +177,112 @@ hx_stream_set_counters(hx_stream_t stream, stream->pc_rx_errors = rx_errors; } +void +hx_stream_reset(hx_stream_t stream) +{ + stream->rx_frame_bytes = 0; + stream->rx_escaped = false; + + stream->tx_buf = NULL; + stream->tx_resid = 0; + stream->tx_state = TX_IDLE; +} + int -hx_stream_send(hx_stream_t stream, +hx_stream_start(hx_stream_t stream, const void *data, size_t count) { - union { - uint8_t b[4]; - uint32_t w; - } u; - const uint8_t *p = (const uint8_t *)data; - unsigned resid = count; - - if (resid > HX_STREAM_MAX_FRAME) + if (count > HX_STREAM_MAX_FRAME) return -EINVAL; - /* start the frame */ - hx_tx_raw(stream, FBO); + stream->tx_buf = data; + stream->tx_resid = count; + stream->tx_state = TX_SEND_START; + stream->tx_crc = crc32(data, count); + return OK; +} + +int +hx_stream_send_next(hx_stream_t stream) +{ + int c; + + /* sort out what we're going to send */ + switch (stream->tx_state) { - /* transmit the data */ - while (resid--) - hx_tx_byte(stream, *p++); + case TX_SEND_START: + stream->tx_state = TX_SEND_DATA; + return FBO; - /* compute the CRC */ - u.w = crc32(data, count); + case TX_SEND_DATA: + c = *stream->tx_buf; - /* send the CRC */ - p = &u.b[0]; - resid = 4; + switch (c) { + case FBO: + case CEO: + stream->tx_state = TX_SENT_ESCAPE; + return CEO; + } + break; + + case TX_SENT_ESCAPE: + c = *stream->tx_buf ^ 0x20; + stream->tx_state = TX_SEND_DATA; + break; + + case TX_SEND_END: + stream->tx_state = TX_IDLE; + return FBO; + + case TX_IDLE: + default: + return -1; + } + + /* if we are here, we have consumed a byte from the buffer */ + stream->tx_resid--; + stream->tx_buf++; + + /* buffer exhausted */ + if (stream->tx_resid == 0) { + uint8_t *pcrc = (uint8_t *)&stream->tx_crc; + + /* was the buffer the frame CRC? */ + if (stream->tx_buf == (pcrc + sizeof(stream->tx_crc))) { + stream->tx_state = TX_SEND_END; + } else { + /* no, it was the payload - switch to sending the CRC */ + stream->tx_buf = pcrc; + stream->tx_resid = sizeof(stream->tx_crc); + } + } + return c; +} + +int +hx_stream_send(hx_stream_t stream, + const void *data, + size_t count) +{ + int result; - while (resid--) - hx_tx_byte(stream, *p++); + result = hx_stream_start(stream, data, count); + if (result != OK) + return result; - /* and the trailing frame separator */ - hx_tx_raw(stream, FBO); + int c; + while ((c = hx_stream_send_next(stream)) >= 0) + hx_tx_raw(stream, c); /* check for transmit error */ - if (stream->txerror) { - stream->txerror = false; + if (stream->tx_error) { + stream->tx_error = false; return -EIO; } perf_count(stream->pc_tx_frames); - return 0; + return OK; } void @@ -234,17 +295,17 @@ hx_stream_rx(hx_stream_t stream, uint8_t c) } /* escaped? */ - if (stream->escaped) { - stream->escaped = false; + if (stream->rx_escaped) { + stream->rx_escaped = false; c ^= 0x20; } else if (c == CEO) { - /* now escaped, ignore the byte */ - stream->escaped = true; + /* now rx_escaped, ignore the byte */ + stream->rx_escaped = true; return; } /* save for later */ - if (stream->frame_bytes < sizeof(stream->buf)) - stream->buf[stream->frame_bytes++] = c; + if (stream->rx_frame_bytes < sizeof(stream->rx_buf)) + stream->rx_buf[stream->rx_frame_bytes++] = c; } diff --git a/src/modules/systemlib/hx_stream.h b/src/modules/systemlib/hx_stream.h index 128689953..1f3927222 100644 --- a/src/modules/systemlib/hx_stream.h +++ b/src/modules/systemlib/hx_stream.h @@ -58,7 +58,8 @@ __BEGIN_DECLS * Allocate a new hx_stream object. * * @param fd The file handle over which the protocol will - * communicate. + * communicate, or -1 if the protocol will use + * hx_stream_start/hx_stream_send_next. * @param callback Called when a frame is received. * @param callback_arg Passed to the callback. * @return A handle to the stream, or NULL if memory could @@ -80,6 +81,7 @@ __EXPORT extern void hx_stream_free(hx_stream_t stream); * * Any counter may be set to NULL to disable counting that datum. * + * @param stream A handle returned from hx_stream_init. * @param tx_frames Counter for transmitted frames. * @param rx_frames Counter for received frames. * @param rx_errors Counter for short and corrupt received frames. @@ -90,6 +92,44 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream, perf_counter_t rx_errors); /** + * Reset a stream. + * + * Forces the local stream state to idle. + * + * @param stream A handle returned from hx_stream_init. + */ +__EXPORT extern void hx_stream_reset(hx_stream_t stream); + +/** + * Prepare to send a frame. + * + * Use this in conjunction with hx_stream_send_next to + * set the frame to be transmitted. + * + * Use hx_stream_send() to write to the stream fd directly. + * + * @param stream A handle returned from hx_stream_init. + * @param data Pointer to the data to send. + * @param count The number of bytes to send. + * @return Zero on success, -errno on error. + */ +__EXPORT extern int hx_stream_start(hx_stream_t stream, + const void *data, + size_t count); + +/** + * Get the next byte to send for a stream. + * + * This requires that the stream be prepared for sending by + * calling hx_stream_start first. + * + * @param stream A handle returned from hx_stream_init. + * @return The byte to send, or -1 if there is + * nothing left to send. + */ +__EXPORT extern int hx_stream_send_next(hx_stream_t stream); + +/** * Send a frame. * * This function will block until all frame bytes are sent if diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fd0289c9a..b470c1227 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -47,4 +47,5 @@ SRCS = err.c \ pid/pid.c \ geo/geo.c \ systemlib.c \ - airspeed.c + airspeed.c \ + system_params.c diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 879f4715a..3c1e10287 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -201,23 +201,50 @@ perf_end(perf_counter_t handle) switch (handle->type) { case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - pce->event_count++; - pce->time_total += elapsed; + if (pce->time_start != 0) { + hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; - if ((pce->time_least > elapsed) || (pce->time_least == 0)) - pce->time_least = elapsed; + pce->event_count++; + pce->time_total += elapsed; - if (pce->time_most < elapsed) - pce->time_most = elapsed; + if ((pce->time_least > elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < elapsed) + pce->time_most = elapsed; + + pce->time_start = 0; + } + } + break; + + default: + break; + } +} + +void +perf_cancel(perf_counter_t handle) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + pce->time_start = 0; } + break; default: break; } } + + void perf_reset(perf_counter_t handle) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 5c2cb15b2..4cd8b67a1 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -92,13 +92,25 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. */ __EXPORT extern void perf_end(perf_counter_t handle); /** - * Reset a performance event. + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +__EXPORT extern void perf_cancel(perf_counter_t handle); + +/** + * Reset a performance counter. * * This call resets performance counter to initial state * diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c new file mode 100644 index 000000000..75be090f8 --- /dev/null +++ b/src/modules/systemlib/system_params.c @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 system_params.c + * + * System wide parameters + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +// Auto-start script with index #n +PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); + +// Automatically configure default values +PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61..301cfa255 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -37,6 +37,10 @@ * Common object definitions without a better home. */ +/** + * @defgroup topics List of all uORB topics. + */ + #include <nuttx/config.h> #include <drivers/drv_orb_dev.h> diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..a27095be5 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,11 +52,20 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index 088c4fc8f..d7b404ad4 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -53,11 +53,20 @@ #define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_effective_s { uint64_t timestamp; float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_effective_0); ORB_DECLARE(actuator_controls_effective_1); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index bbe429073..30895ca83 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -52,12 +52,21 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ int noutputs; /**< valid outputs */ }; +/** + * @} + */ + /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs_0); ORB_DECLARE(actuator_outputs_1); diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h index a9d1b83fd..9253c787d 100644 --- a/src/modules/uORB/topics/debug_key_value.h +++ b/src/modules/uORB/topics/debug_key_value.h @@ -47,6 +47,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index e67a39e1e..11332d7a7 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -52,10 +52,6 @@ #include "../uORB.h" /** - * @addtogroup topics @{ - */ - -/** * The number of ESCs supported. * Current (Q2/2013) we support 8 ESCs, */ @@ -63,7 +59,8 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ - ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */ + ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { @@ -75,7 +72,12 @@ enum ESC_CONNECTION_TYPE { }; /** - * + * @addtogroup topics + * @{ + */ + +/** + * Electronic speed controller status. */ struct esc_status_s { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 253f444b3..978a3383a 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum NAV_CMD { NAV_CMD_WAYPOINT = 0, NAV_CMD_LOITER_TURN_COUNT, @@ -62,6 +57,11 @@ enum NAV_CMD { }; /** + * @addtogroup topics + * @{ + */ + +/** * Global position setpoint in WGS84 coordinates. * * This is the position the MAV is heading towards. If it of type loiter, diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2e895c59c..7901b930a 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -44,11 +44,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Off-board control inputs. * * Typically sent by a ground control station / joystick or by @@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; +/** + * @addtogroup topics + * @{ + */ + struct offboard_control_setpoint_s { uint64_t timestamp; diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h index 8f4be3b3f..a6ad8a131 100644 --- a/src/modules/uORB/topics/omnidirectional_flow.h +++ b/src/modules/uORB/topics/omnidirectional_flow.h @@ -46,6 +46,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 300e895c7..68964deb0 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -42,11 +42,20 @@ #include <stdint.h> #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + struct parameter_update_s { /** time at which the latest parameter was updated */ uint64_t timestamp; }; +/** + * @} + */ + ORB_DECLARE(parameter_update); #endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 9dd54df91..e69335b3d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, * leaving at a sane value of 14. @@ -83,6 +78,11 @@ enum RC_CHANNELS_FUNCTION RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; +/** + * @addtogroup topics + * @{ + */ + struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 9a76b5182..ad164555e 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum MAGNETOMETER_MODE { MAGNETOMETER_MODE_NORMAL = 0, MAGNETOMETER_MODE_POSITIVE_BIAS, @@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE { }; /** + * @addtogroup topics + * @{ + */ + +/** * Sensor readings in raw and SI-unit form. * * These values are read from the sensors. Raw values are in sensor-specific units, diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index c415e832e..cfe0bf69e 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,10 +50,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - */ - enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, @@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE }; /** + * @addtogroup topics + */ + +/** * State of individual sub systems */ struct subsystem_info_s { diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index f30852de5..828fb31cc 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE { TELEMETRY_STATUS_RADIO_TYPE_WIRE }; +/** + * @addtogroup topics + * @{ + */ + struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ @@ -62,6 +67,10 @@ struct telemetry_status_s { uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; +/** + * @} + */ + ORB_DECLARE(telemetry_status); #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index c31c81d0c..4380a5ee7 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -48,6 +48,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index fac571659..31ff014de 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Commands for commander app. * * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, @@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ }; +/** + * @addtogroup topics + * @{ + */ struct vehicle_command_s { diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 318abba89..8516b263f 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -60,8 +60,8 @@ */ struct vehicle_global_position_set_triplet_s { - bool previous_valid; - bool next_valid; + bool previous_valid; /**< flag indicating previous position is valid */ + bool next_valid; /**< flag indicating next position is valid */ struct vehicle_global_position_setpoint_s previous; struct vehicle_global_position_setpoint_s current; diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 3ae3ff28c..5c8ce1e4d 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ float param1; float param2; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f6..94068a9ac 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,10 +54,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics @{ - */ - /* State Machine */ typedef enum { @@ -137,6 +133,10 @@ enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ }; +/** + * @addtogroup topics + * @{ + */ /** * state machine / state of vehicle. diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c new file mode 100644 index 000000000..5a02fd620 --- /dev/null +++ b/src/systemcmds/config/config.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 config.c + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * config tool. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/stat.h> + +#include <arch/board/board.h> + +#include <drivers/drv_gyro.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_mag.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" + +__EXPORT int config_main(int argc, char *argv[]); + +static void do_gyro(int argc, char *argv[]); +static void do_accel(int argc, char *argv[]); +static void do_mag(int argc, char *argv[]); + +int +config_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "gyro")) { + if (argc >= 3) { + do_gyro(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + + if (!strcmp(argv[1], "accel")) { + if (argc >= 3) { + do_accel(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + + if (!strcmp(argv[1], "mag")) { + if (argc >= 3) { + do_mag(argc - 2, argv + 2); + } else { + errx(1, "not enough parameters."); + } + } + } + + errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); +} + +static void +do_gyro(int argc, char *argv[]) +{ + int fd; + + fd = open(GYRO_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", GYRO_DEVICE_PATH); + errx(1, "FATAL: no gyro found"); + + } else { + + if (argc >= 2) { + + char* end; + int i = strtol(argv[1],&end,10); + + if (!strcmp(argv[0], "sampling")) { + + /* set the accel internal sampling rate up to at leat i Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, i); + + } else if (!strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, i); + } else if (!strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ioctl(fd, GYROIOCSRANGE, i); + } + + } else if (argc > 0) { + + if(!strcmp(argv[0], "check")) { + int ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret) { + warnx("gyro self test FAILED! Check calibration:"); + struct gyro_scale scale; + ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("gyro calibration and self test OK"); + } + } + + } else { + warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); + } + + int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = ioctl(fd, GYROIOCGRANGE, 0); + + warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + + close(fd); + } + + exit(0); +} + +static void +do_mag(int argc, char *argv[]) +{ + int fd; + + fd = open(MAG_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + + } else { + + if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret) { + warnx("mag self test FAILED! Check calibration."); + struct mag_scale scale; + ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("mag calibration and self test OK"); + } + } + + } else { + warnx("no arguments given. Try: \n\n\t'check' or 'info'\n\t"); + } + + int srate = -1;//ioctl(fd, MAGIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = -1;//ioctl(fd, MAGIOCGRANGE, 0); + + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); + + close(fd); + } + + exit(0); +} + +static void +do_accel(int argc, char *argv[]) +{ + int fd; + + fd = open(ACCEL_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", ACCEL_DEVICE_PATH); + errx(1, "FATAL: no accelerometer found"); + + } else { + + if (argc >= 2) { + + char* end; + int i = strtol(argv[1],&end,10); + + if (!strcmp(argv[0], "sampling")) { + + /* set the accel internal sampling rate up to at leat i Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, i); + + } else if (!strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, i); + } else if (!strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ioctl(fd, ACCELIOCSRANGE, i); + } + } else if (argc > 0) { + + if (!strcmp(argv[0], "check")) { + int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret) { + warnx("accel self test FAILED! Check calibration."); + struct accel_scale scale; + ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); + warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset); + warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale); + } else { + warnx("accel calibration and self test OK"); + } + } + + } else { + warnx("no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 2 G\n\t"); + } + + int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = ioctl(fd, ACCELIOCGRANGE, 0); + + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d m/s", srate, prate, range); + + close(fd); + } + + exit(0); +} diff --git a/src/systemcmds/config/module.mk b/src/systemcmds/config/module.mk new file mode 100644 index 000000000..0a75810b0 --- /dev/null +++ b/src/systemcmds/config/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2012, 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 the config tool. +# + +MODULE_COMMAND = config +SRCS = config.c + +MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os + diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 60e61d07b..40a9297a7 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,6 +63,7 @@ static void do_import(const char* param_file_name); static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val); +static void do_compare(const char* name, const char* val); int param_main(int argc, char *argv[]) @@ -117,9 +118,17 @@ param_main(int argc, char *argv[]) errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'"); } } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 4) { + do_compare(argv[2], argv[3]); + } else { + errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'"); + } + } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); } static void @@ -295,3 +304,65 @@ do_set(const char* name, const char* val) exit(0); } + +static void +do_compare(const char* name, const char* val) +{ + int32_t i; + float f; + param_t param = param_find(name); + + /* set nothing if parameter cannot be found */ + if (param == PARAM_INVALID) { + /* param not found */ + errx(1, "Error: Parameter %s not found.", name); + } + + /* + * Set parameter if type is known and conversion from string to value turns out fine + */ + + int ret = 1; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + if (!param_get(param, &i)) { + + /* convert string */ + char* end; + int j = strtol(val,&end,10); + if (i == j) { + printf(" %d: ", i); + ret = 0; + } + + } + + break; + + case PARAM_TYPE_FLOAT: + if (!param_get(param, &f)) { + + /* convert string */ + char* end; + float g = strtod(val, &end); + if (fabsf(f - g) < 1e-7f) { + printf(" %4.4f: ", (double)f); + ret = 0; + } + } + + break; + + default: + errx(1, "<unknown / unsupported type %d>\n", 0 + param_type(param)); + } + + if (ret == 0) { + printf("%c %s: equal\n", + param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), + param_name(param)); + } + + exit(ret); +} diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 7752ffe67..d1dd85d47 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -135,6 +135,7 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(BARO_DEVICE_PATH, 0); + close(fd); /* ---- RC CALIBRATION ---- */ @@ -251,6 +252,11 @@ system_eval: int buzzer = open("/dev/tone_alarm", O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); + if (leds < 0) { + close(buzzer); + errx(1, "failed to open leds, aborting"); + } + /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk new file mode 100644 index 000000000..e4eb1d143 --- /dev/null +++ b/src/systemcmds/ramtron/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = ramtron +SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c new file mode 100644 index 000000000..03c713987 --- /dev/null +++ b/src/systemcmds/ramtron/ramtron.c @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 ramtron.c + * + * ramtron service and utility app. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/spi.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +__EXPORT int ramtron_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD_RAMTRON + +/* create a fake command with decent message to not confuse users */ +int ramtron_main(int argc, char *argv[]) +{ + errx(1, "RAMTRON not enabled, skipping."); +} +#else + +static void ramtron_attach(void); +static void ramtron_start(void); +static void ramtron_erase(void); +static void ramtron_ioctl(unsigned operation); +static void ramtron_save(const char *name); +static void ramtron_load(const char *name); +static void ramtron_test(void); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *ramtron_mtd; + +int ramtron_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) + ramtron_start(); + + if (!strcmp(argv[1], "save_param")) + ramtron_save(argv[2]); + + if (!strcmp(argv[1], "load_param")) + ramtron_load(argv[2]); + + if (!strcmp(argv[1], "erase")) + ramtron_erase(); + + if (!strcmp(argv[1], "test")) + ramtron_test(); + + if (0) { /* these actually require a file on the filesystem... */ + + if (!strcmp(argv[1], "reformat")) + ramtron_ioctl(FIOC_REFORMAT); + + if (!strcmp(argv[1], "repack")) + ramtron_ioctl(FIOC_OPTIMIZE); + } + } + + errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + + +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + // xxx set in ramtron driver, leave this out +// SPI_SETFREQUENCY(spi, 4000000); + SPI_SETFREQUENCY(spi, 375000000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + ramtron_mtd = ramtron_initialize(spi); + if (ramtron_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: ramtron needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (ramtron_mtd == NULL) + errx(1, "failed to initialize ramtron driver"); + + attached = true; +} + +static void +ramtron_start(void) +{ + int ret; + + if (started) + errx(1, "ramtron already mounted"); + + if (!attached) + ramtron_attach(); + + /* start NXFFS */ + ret = nxffs_initialize(ramtron_mtd); + + if (ret < 0) + errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); + + /* mount the ramtron */ + ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); + + if (ret < 0) + errx(1, "failed to mount /ramtron - erase ramtron to reformat"); + + started = true; + warnx("mounted ramtron at /ramtron"); + exit(0); +} + +//extern int at24c_nuke(void); + +static void +ramtron_erase(void) +{ + if (!attached) + ramtron_attach(); + +// if (at24c_nuke()) + errx(1, "erase failed"); + + errx(0, "erase done, reboot now"); +} + +static void +ramtron_ioctl(unsigned operation) +{ + int fd; + + fd = open("/ramtron/.", 0); + + if (fd < 0) + err(1, "open /ramtron"); + + if (ioctl(fd, operation, 0) < 0) + err(1, "ioctl"); + + exit(0); +} + +static void +ramtron_save(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); + + /* delete the file in case it exists */ + unlink(name); + + /* create the file */ + int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) + err(1, "opening '%s' failed", name); + + int result = param_export(fd, false); + close(fd); + + if (result < 0) { + unlink(name); + errx(1, "error exporting to '%s'", name); + } + + exit(0); +} + +static void +ramtron_load(const char *name) +{ + if (!started) + errx(1, "must be started first"); + + if (!name) + err(1, "missing argument for device name, try '/ramtron/parameters'"); + + warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); + + int fd = open(name, O_RDONLY); + + if (fd < 0) + err(1, "open '%s'", name); + + int result = param_load(fd); + close(fd); + + if (result < 0) + errx(1, "error importing from '%s'", name); + + exit(0); +} + +//extern void at24c_test(void); + +static void +ramtron_test(void) +{ +// at24c_test(); + exit(0); +} + +#endif diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index ab536d956..62a873270 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[]) int fd; int ret = 0; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f21dd115b..f6e540401 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -94,7 +94,7 @@ extern uint16_t ppm_pulse_history[]; int test_ppm(int argc, char *argv[]) { -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL unsigned i; printf("channels: %u\n", ppm_decoded_channels); |
