diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-08 21:56:11 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-08 21:56:11 +0400 |
| commit | e8224376ca4d32e948cbee75bfecf8a30f3e98ea (patch) | |
| tree | 65290003878c65247d99ad21849a6f3cbeac2e35 | |
| parent | 28bf8e238e35a7bbba81f86e63cd0a49226673a4 (diff) | |
| parent | c63995e91c188b476aa2608b42a366f68dced423 (diff) | |
| download | px4-firmware-e8224376ca4d32e948cbee75bfecf8a30f3e98ea.tar.gz px4-firmware-e8224376ca4d32e948cbee75bfecf8a30f3e98ea.tar.bz2 px4-firmware-e8224376ca4d32e948cbee75bfecf8a30f3e98ea.zip | |
Merge branch 'master' into vector_control
636 files changed, 40407 insertions, 10462 deletions
diff --git a/.gitignore b/.gitignore index 3fdc1bf2a..3e94cf620 100644 --- a/.gitignore +++ b/.gitignore @@ -19,12 +19,19 @@ Archives/* Build/* core cscope.out -dot.gdbinit Firmware.sublime-workspace Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ +/nuttx-configs/px4io-v2/src/.depend +/nuttx-configs/px4io-v2/src/Make.dep +/nuttx-configs/px4io-v2/src/libboard.a +/nuttx-configs/px4io-v1/src/.depend +/nuttx-configs/px4io-v1/src/Make.dep +/nuttx-configs/px4io-v1/src/libboard.a /NuttX /Documentation/doxy.log /Documentation/html/ -/Documentation/doxygen*objdb*tmp
\ No newline at end of file +/Documentation/doxygen*objdb*tmp +.tags +.tags_sorted_by_file diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 803d96453..4c539b345 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -56,7 +56,7 @@ define vecstate if $mmfsr & (1<<3) printf " during exception return" end - if $mmfsr & (1<<0) + if $mmfsr & (1<<1) printf " during data access" end if $mmfsr & (1<<0) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index b0864a229..093edd0e0 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -2,6 +2,106 @@ import gdb, gdb.types +class NX_register_set(object): + """Copy of the registers for a given context""" + + v7_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 11, + 'R1': 12, + 'R2': 13, + 'R3': 14, + 'R12': 15, + 'R14': 16, + 'LR': 16, + 'R15': 17, + 'PC': 17, + 'XPSR': 18, + } + + v7em_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 27, + 'R1': 28, + 'R2': 29, + 'R3': 30, + 'R12': 31, + 'R14': 32, + 'LR': 32, + 'R15': 33, + 'PC': 33, + 'XPSR': 34, + } + + regs = dict() + + def __init__(self, xcpt_regs): + if xcpt_regs is None: + self.regs['R0'] = long(gdb.parse_and_eval('$r0')) + self.regs['R1'] = long(gdb.parse_and_eval('$r1')) + self.regs['R2'] = long(gdb.parse_and_eval('$r2')) + self.regs['R3'] = long(gdb.parse_and_eval('$r3')) + self.regs['R4'] = long(gdb.parse_and_eval('$r4')) + self.regs['R5'] = long(gdb.parse_and_eval('$r5')) + self.regs['R6'] = long(gdb.parse_and_eval('$r6')) + self.regs['R7'] = long(gdb.parse_and_eval('$r7')) + self.regs['R8'] = long(gdb.parse_and_eval('$r8')) + self.regs['R9'] = long(gdb.parse_and_eval('$r9')) + self.regs['R10'] = long(gdb.parse_and_eval('$r10')) + self.regs['R11'] = long(gdb.parse_and_eval('$r11')) + self.regs['R12'] = long(gdb.parse_and_eval('$r12')) + self.regs['R13'] = long(gdb.parse_and_eval('$r13')) + self.regs['SP'] = long(gdb.parse_and_eval('$sp')) + self.regs['R14'] = long(gdb.parse_and_eval('$r14')) + self.regs['LR'] = long(gdb.parse_and_eval('$lr')) + self.regs['R15'] = long(gdb.parse_and_eval('$r15')) + self.regs['PC'] = long(gdb.parse_and_eval('$pc')) + self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + else: + for key in self.v7em_regmap.keys(): + self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + + + @classmethod + def with_xcpt_regs(cls, xcpt_regs): + return cls(xcpt_regs) + + @classmethod + def for_current(cls): + return cls(None) + + def __format__(self, format_spec): + return format_spec.format( + registers = self.registers + ) + + @property + def registers(self): + return self.regs + + class NX_task(object): """Reference to a NuttX task and methods for introspecting it""" @@ -139,56 +239,12 @@ class NX_task(object): 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 + registers = NX_register_set.for_current().registers 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]) + registers = NX_register_set.with_xcpt_regs(regs).registers + self.__dict__['registers'] = registers return self.__dict__['registers'] @@ -267,8 +323,7 @@ class NX_show_heap (gdb.Command): def _print_allocations(self, region_start, region_end): if region_start >= region_end: - print 'heap region {} corrupt'.format(hex(region_start)) - return + raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start print 'heap {} - {}'.format(region_start, region_end) cursor = 1 @@ -286,13 +341,31 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print "{} heap(s)".format(nregions) + 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() +class NX_show_interrupted_thread (gdb.Command): + """(NuttX) prints the register state of an interrupted thread when in interrupt/exception context""" + def __init__(self): + super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER) + def invoke(self, args, from_tty): + regs = gdb.lookup_global_symbol('current_regs').value() + if regs is 0: + raise gdb.GdbError('not in interrupt context') + else: + registers = NX_register_set.with_xcpt_regs(regs) + my_fmt = '' + 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(registers, my_fmt) +NX_show_interrupted_thread() @@ -27,7 +27,7 @@ define _perf_print # PC_COUNT if $hdr->type == 0 set $count = (struct perf_ctr_count *)$hdr - printf "%llu events,\n", $count->event_count; + printf "%llu events\n", $count->event_count end # PC_ELPASED if $hdr->type == 1 diff --git a/Debug/dot.gdbinit b/Debug/dot.gdbinit new file mode 100644 index 000000000..d70410bc7 --- /dev/null +++ b/Debug/dot.gdbinit @@ -0,0 +1,13 @@ +# copy the file to .gdbinit in your Firmware tree, and adjust the path +# below to match your system +# For example: +# target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE5A1C4-if00 +# target extended /dev/ttyACM4 + + +monitor swdp_scan +attach 1 +monitor vector_catch disable hard +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 diff --git a/Debug/olimex-px4fmu-debug.cfg b/Debug/olimex-px4fmu-debug.cfg new file mode 100644 index 000000000..61d70070d --- /dev/null +++ b/Debug/olimex-px4fmu-debug.cfg @@ -0,0 +1,22 @@ +# program a bootable device load on a mavstation +# To run type openocd -f mavprogram.cfg + +source [find interface/olimex-arm-usb-ocd-h.cfg] +source [find px4fmu-v1-board.cfg] + +init +halt + +# Find the flash inside this CPU +flash probe 0 + +# erase it (128 pages) then program and exit + +#flash erase_sector 0 0 127 +# stm32f1x mass_erase 0 + +# It seems that Pat's image has a start address offset of 0x1000 but the vectors need to be at zero, so fixbin.sh moves things around +#flash write_bank 0 fixed.bin 0 +#flash write_image firmware.elf +#shutdown + diff --git a/Debug/openocd.gdbinit b/Debug/openocd.gdbinit new file mode 100644 index 000000000..92d78b58d --- /dev/null +++ b/Debug/openocd.gdbinit @@ -0,0 +1,21 @@ +target remote :3333 + +# Don't let GDB get confused while stepping +define hook-step + mon cortex_m maskisr on +end +define hookpost-step + mon cortex_m maskisr off +end + +mon init +mon stm32_init +# mon reset halt +mon poll +mon cortex_m maskisr auto +set mem inaccessible-by-default off +set print pretty +source Debug/PX4 + +echo PX4 resumed, press ctrl-c to interrupt\n +continue diff --git a/Debug/px4fmu-v1-board.cfg b/Debug/px4fmu-v1-board.cfg new file mode 100644 index 000000000..19b862a2d --- /dev/null +++ b/Debug/px4fmu-v1-board.cfg @@ -0,0 +1,38 @@ +# The latest defaults in OpenOCD 0.7.0 are actually prettymuch correct for the px4fmu + +# increase working area to 32KB for faster flash programming +set WORKAREASIZE 0x8000 + +source [find target/stm32f4x.cfg] + +# needed for px4 +reset_config trst_only + +proc stm32_reset {} { + reset halt +# FIXME - needed to init periphs on reset +# 0x40023800 RCC base +# 0x24 RCC_APB2 0x75933 +# RCC_APB2 0 +} + +# perform init that is required on each connection to the target +proc stm32_init {} { + + # force jtag to not shutdown during sleep + #uint32_t cr = getreg32(STM32_DBGMCU_CR); + #cr |= DBGMCU_CR_STANDBY | DBGMCU_CR_STOP | DBGMCU_CR_SLEEP; + #putreg32(cr, STM32_DBGMCU_CR); + mww 0xe0042004 00000007 +} + +# if srst is not fitted use SYSRESETREQ to +# perform a soft reset +cortex_m reset_config sysresetreq + +# Let GDB directly program elf binaries +gdb_memory_map enable + +# doesn't work yet +gdb_flash_program disable + diff --git a/Debug/runopenocd.sh b/Debug/runopenocd.sh new file mode 100755 index 000000000..6258fccfb --- /dev/null +++ b/Debug/runopenocd.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +openocd -f interface/olimex-arm-usb-ocd-h.cfg -f $DIR/px4fmu-v1-board.cfg diff --git a/Debug/stm32f4x.cfg b/Debug/stm32f4x.cfg deleted file mode 100644 index 28bfcfbbb..000000000 --- a/Debug/stm32f4x.cfg +++ /dev/null @@ -1,64 +0,0 @@ -# script for stm32f2xxx - -if { [info exists CHIPNAME] } { - set _CHIPNAME $CHIPNAME -} else { - set _CHIPNAME stm32f4xxx -} - -if { [info exists ENDIAN] } { - set _ENDIAN $ENDIAN -} else { - set _ENDIAN little -} - -# Work-area is a space in RAM used for flash programming -# By default use 64kB -if { [info exists WORKAREASIZE] } { - set _WORKAREASIZE $WORKAREASIZE -} else { - set _WORKAREASIZE 0x10000 -} - -# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz -# -# Since we may be running of an RC oscilator, we crank down the speed a -# bit more to be on the safe side. Perhaps superstition, but if are -# running off a crystal, we can run closer to the limit. Note -# that there can be a pretty wide band where things are more or less stable. -jtag_khz 1000 - -jtag_nsrst_delay 100 -jtag_ntrst_delay 100 - -#jtag scan chain -if { [info exists CPUTAPID ] } { - set _CPUTAPID $CPUTAPID -} else { - # See STM Document RM0033 - # Section 32.6.3 - corresponds to Cortex-M3 r2p0 - set _CPUTAPID 0x4ba00477 -} -jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID - -if { [info exists BSTAPID ] } { - set _BSTAPID $BSTAPID -} else { - # See STM Document RM0033 - # Section 32.6.2 - # - set _BSTAPID 0x06413041 -} -jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID - -set _TARGETNAME $_CHIPNAME.cpu -target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto - -$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 - -set _FLASHNAME $_CHIPNAME.flash -flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME - -# if srst is not fitted use SYSRESETREQ to -# perform a soft reset -cortex_m3 reset_config sysresetreq 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/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg Binary files differnew file mode 100644 index 000000000..0918edcac --- /dev/null +++ b/Documentation/fixed_wing_control.odg diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf Binary files differindex cd234ada7..af41953ee 100644 --- a/Documentation/flight_mode_state_machine.pdf +++ b/Documentation/flight_mode_state_machine.pdf diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg Binary files differindex a8a6f93f3..29d738c39 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 3141eed7f..856dd55c5 100644 --- a/Documentation/rc_mode_switch.pdf +++ b/Documentation/rc_mode_switch.pdf diff --git a/Images/px4fmu-v2.prototype b/Images/px4fmu-v2.prototype new file mode 100644 index 000000000..5109b77d1 --- /dev/null +++ b/Images/px4fmu-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 9, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv2 board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv2", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Images/px4io-v2.prototype b/Images/px4io-v2.prototype new file mode 100644 index 000000000..af87924e9 --- /dev/null +++ b/Images/px4io-v2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/Images/px4iov2.prototype b/Images/px4iov2.prototype new file mode 100644 index 000000000..af87924e9 --- /dev/null +++ b/Images/px4iov2.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 10, + "magic": "PX4FWv2", + "description": "Firmware for the PX4IOv2 board", + "image": "", + "build_time": 0, + "summary": "PX4IOv2", + "version": "2.0", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} @@ -40,14 +40,27 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk # -# Canned firmware configurations that we build. +# Get a version string provided by git +# This assumes that git command is available and that +# the directory holding this file also contains .git directory # -CONFIGS ?= $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +GIT_DESC := $(shell git log -1 --pretty=format:%H) +ifneq ($(words $(GIT_DESC)),1) + GIT_DESC := "unknown_git_version" +endif +export GIT_DESC + +# +# Canned firmware configurations that we (know how to) build. +# +KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) +CONFIGS ?= $(KNOWN_CONFIGS) # -# Boards that we build NuttX export kits for. +# Boards that we (know how to) build NuttX export kits for. # -BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +KNOWN_BOARDS := $(subst board_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)board_*.mk)))) +BOARDS ?= $(KNOWN_BOARDS) # # Debugging @@ -87,10 +100,11 @@ endif # # Built products # -STAGED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) -FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) +DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4) +STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4) +FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4) -all: $(STAGED_FIRMWARES) +all: $(DESIRED_FIRMWARES) # # Copy FIRMWARES into the image directory. @@ -114,14 +128,27 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4: @$(ECHO) %%%% @$(ECHO) %%%% Building $(config) in $(work_dir) @$(ECHO) %%%% - $(Q) mkdir -p $(work_dir) - $(Q) make -r -C $(work_dir) \ + $(Q) $(MKDIR) -p $(work_dir) + $(Q) $(MAKE) -r -C $(work_dir) \ -f $(PX4_MK_DIR)firmware.mk \ CONFIG=$(config) \ WORK_DIR=$(work_dir) \ $(FIRMWARE_GOAL) # +# Make FMU firmwares depend on the corresponding IO firmware. +# +# This is a pretty vile hack, since it hard-codes knowledge of the FMU->IO dependency +# and forces the _default config in all cases. There has to be a better way to do this... +# +FMU_VERSION = $(patsubst px4fmu-%,%,$(word 1, $(subst _, ,$(1)))) +define FMU_DEP +$(BUILD_DIR)$(1).build/firmware.px4: $(IMAGE_DIR)px4io-$(call FMU_VERSION,$(1))_default.px4 +endef +FMU_CONFIGS := $(filter px4fmu%,$(CONFIGS)) +$(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config)))) + +# # Build the NuttX export archives. # # Note that there are no explicit dependencies extended from these @@ -147,12 +174,12 @@ $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -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) CONFIG_ARCH_BOARD=$(board) export - $(Q) mkdir -p $(dir $@) + $(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)) @@ -168,11 +195,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) make -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -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 + $(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 @@ -191,7 +218,7 @@ $(NUTTX_SRC): # Testing targets # testbuild: - $(Q) (cd $(PX4_BASE) && make distclean && make archives && make -j8) + $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) # # Cleanup targets. 'clean' should remove all built products and force @@ -206,7 +233,8 @@ clean: .PHONY: distclean distclean: clean $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # # Print some help text @@ -228,9 +256,9 @@ help: @$(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 ""; \ + $(ECHO) " $$config"; \ + $(ECHO) " Build just the $$config firmware configuration."; \ + $(ECHO) ""; \ done @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." diff --git a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x b/ROMFS/px4fmu_common/init.d/01_fmu_quad_x deleted file mode 100644 index 58a970eba..000000000 --- a/ROMFS/px4fmu_common/init.d/01_fmu_quad_x +++ /dev/null @@ -1,107 +0,0 @@ -#!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 deleted file mode 100644 index c63e92f6d..000000000 --- a/ROMFS/px4fmu_common/init.d/02_io_quad_x +++ /dev/null @@ -1,101 +0,0 @@ -#!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/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f55ac2ae3..f6f09ac22 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -1,99 +1,57 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# - -# Disable the USB interface -set USB no -# Disable autostarting other apps -set MODE ardrone - -echo "[init] doing PX4IOAR startup..." - -# -# Start the ORB -# -uorb start +echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" # -# Load microSD params +# Load default params for this platform # -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # -# Configure PX4FMU for operation with PX4IOAR -# -fmu mode_gpio_serial - -# -# Start the sensors. -# -sh /etc/init.d/rc.sensors - -# # Start MAVLink # mavlink start -d /dev/ttyS0 -b 57600 usleep 5000 - -# -# Start the commander. -# -commander start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - + # -# Fire up the multi rotor attitude controller +# Configure PX4FMU for operation with PX4IOAR # -multirotor_att_control start +fmu mode_gpio_serial # # Fire up the AR.Drone interface. # ardrone_interface start -d /dev/ttyS1 - -# -# Start logging -# -sdlog start -s 10 - -# -# Start GPS capture -# -gps start - -# -# Start system state -# -if blinkm start -then - echo "using BlinkM for state indication" - blinkm systemstate -else - echo "no BlinkM found, OK." -fi # -# startup is done; we don't want the shell because we -# use the same UART for telemetry +# Start common for all multirotors apps # -echo "[init] startup done" - +sh /etc/init.d/rc.multirotor + +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index e7173f6e6..9b739f245 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -1,42 +1,53 @@ #!nsh -# -# Flight startup script for PX4FMU on PX4IOAR carrier board. -# -# Disable the USB interface -set USB no +echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" -# Disable autostarting other apps -set MODE ardrone - -echo "[init] doing PX4IOAR startup..." - # -# Start the ORB +# Load default params for this platform # -uorb start - -# -# Load microSD params -# -echo "[init] loading microSD params" -param select /fs/microsd/params -if [ -f /fs/microsd/params ] +if param compare SYS_AUTOCONFIG 1 then - param load /fs/microsd/params + # Set all params here, then disable autoconfig + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 + + param set SYS_AUTOCONFIG 0 + param save fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 + +# +# Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) +# +mavlink start -d /dev/ttyS0 -b 57600 +mavlink_onboard start -d /dev/ttyS3 -b 115200 +usleep 5000 # # Configure PX4FMU for operation with PX4IOAR # fmu mode_gpio_serial + +# +# Fire up the AR.Drone interface. +# +ardrone_interface start -d /dev/ttyS1 # # Start the sensors. @@ -44,13 +55,6 @@ fmu mode_gpio_serial sh /etc/init.d/rc.sensors # -# Start MAVLink and MAVLink Onboard (Flow Sensor) -# -mavlink start -d /dev/ttyS0 -b 57600 -mavlink_onboard start -d /dev/ttyS3 -b 115200 -usleep 5000 - -# # Start the commander. # commander start @@ -79,16 +83,6 @@ flow_position_control start # Fire up the flow speed controller # flow_speed_control start - -# -# Fire up the AR.Drone interface. -# -ardrone_interface start -d /dev/ttyS1 -# -# startup is done; we don't want the shell because we -# use the same UART for telemetry -# -echo "[init] startup done" - +# Exit, because /dev/ttyS0 is needed for MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil new file mode 100644 index 000000000..40a13b5d1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -0,0 +1,87 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil new file mode 100644 index 000000000..9b664d63e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -0,0 +1,105 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor starting.." + +# +# 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.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.05 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 3.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil new file mode 100644 index 000000000..7b9f41bf6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -0,0 +1,79 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting in state-HIL mode.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil new file mode 100644 index 000000000..0cc07ad34 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -0,0 +1,105 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor + starting.." + +# +# 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.0 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.05 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 3.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil new file mode 100644 index 000000000..344d78422 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -0,0 +1,98 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# 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 we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +set MODE autostart +mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + + +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar new file mode 100644 index 000000000..abe378b22 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -0,0 +1,87 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix +else + echo "Using /etc/mixers/FMU_RET.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler new file mode 100644 index 000000000..c616da988 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -0,0 +1,87 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 new file mode 100644 index 000000000..467b56bbf --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -0,0 +1,97 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330 deleted file mode 100644 index 4450eb50d..000000000 --- a/ROMFS/px4fmu_common/init.d/10_io_f330 +++ /dev/null @@ -1,116 +0,0 @@ -#!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/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 new file mode 100644 index 000000000..818f9375e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -0,0 +1,83 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + commander start + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 + +# +# Start common multirotor apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex new file mode 100644 index 000000000..f83f6cfd0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -0,0 +1,98 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Hex" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 7.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ +# 13 = hexarotor +# +param set MAV_TYPE 13 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_hex_x.mix + +# +# Set PWM output frequency to 400 Hz +# +pwm rate -c 123456 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 123456 -p 900 +pwm min -c 123456 -p 1100 +pwm max -c 123456 -p 1900 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery new file mode 100644 index 000000000..c79e9d283 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -0,0 +1,81 @@ +#!nsh + +echo "[init] Team Blacksheep Discovery Quad" + +# +# 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.006 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.17 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 5.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris new file mode 100644 index 000000000..6be917878 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -0,0 +1,81 @@ +#!nsh + +echo "[init] 3DR Iris Quad" + +# +# 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.004 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 9.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 0.5 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.2 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.0098 + set EXIT_ON_END yes +fi + +# +# Load the mixer for a quad with wide arms +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + +# +# Set PWM output frequency +# +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index 5090b98a4..8a8bc1590 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,26 +1,6 @@ #!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 + +echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" # # Load default params for this platform @@ -28,94 +8,58 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - + +set EXIT_ON_END no + # -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# Start and configure PX4IO or FMU interface # -if [ -f /fs/microsd/px4io.bin ] +if px4io detect 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 + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes 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 - +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + # -# Start system state +# Start common fixedwing apps # -if blinkm start +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] then - blinkm systemstate -fi + exit +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 5090b98a4..63cd7f9b2 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,26 +1,6 @@ #!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 + +echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" # # Load default params for this platform @@ -28,94 +8,80 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 + param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - + +set EXIT_ON_END no + # -# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# Start and configure PX4IO or FMU interface # -if [ -f /fs/microsd/px4io.bin ] +if px4io detect 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 + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes 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 - +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + # -# Start system state +# Start common fixedwing apps # -if blinkm start +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] then - blinkm systemstate -fi + exit +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 new file mode 100644 index 000000000..1e752f13a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -0,0 +1,65 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + # TODO + + param set SYS_AUTOCONFIG 0 + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing +# +param set MAV_TYPE 1 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + + sh /etc/init.d/rc.io + # Limit to 100 Hz updates and (implicit) 50 Hz PWM + px4io limit 100 +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer and start controllers (depends on px4io) +# +if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] +then + echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix +else + echo "Using /etc/mixers/FMU_Q.mix" + mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix +fi + +# +# Start common fixedwing apps +# +sh /etc/init.d/rc.fixedwing + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 5742d685a..fb9dec043 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -1,26 +1,4 @@ #!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 @@ -28,39 +6,18 @@ fi if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig + # TODO + param set SYS_AUTOCONFIG 0 - param save /fs/microsd/params + param save fi # # Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor -# see https://pixhawk.ethz.ch/mavlink/ +# MAV_TYPE 10 = ground rover # param set MAV_TYPE 10 - -# -# 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) # @@ -68,26 +25,16 @@ mavlink start -d /dev/ttyS1 -b 57600 usleep 5000 # -# Start the commander (depends on orb, mavlink) +# Start and configure PX4IO interface # -commander start +sh /etc/init.d/rc.io # -# Start PX4IO interface (depends on orb, commander) -# -px4io start - +# Start the commander (depends on orb, mavlink) # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +commander start # -# Disable px4io topic limiting -# -px4io limit 200 - -# # Start the sensors (depends on orb, px4io) # sh /etc/init.d/rc.sensors @@ -105,18 +52,5 @@ attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -md25 start 3 0x58 +roboclaw start /dev/ttyS2 128 1200 segway 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/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 new file mode 100644 index 000000000..1ee84b9b0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -0,0 +1,83 @@ +#!nsh + +echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" + +# +# 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 +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Set PWM output frequency +# +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1100 +pwm max -c 1234 -p 1900 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test new file mode 100644 index 000000000..f86f4f85b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/cmp_test @@ -0,0 +1,9 @@ +#!nsh + +cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +then + echo "CMP returned true" +else + echo "CMP returned false" +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.boarddetect b/ROMFS/px4fmu_common/init.d/rc.boarddetect deleted file mode 100644 index f233e51df..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.boarddetect +++ /dev/null @@ -1,66 +0,0 @@ -#!nsh -# -# If we are still in flight mode, work out what airframe -# configuration we have and start up accordingly. -# -if [ $MODE != autostart ] -then - echo "[init] automatic startup cancelled by user script" -else - echo "[init] detecting attached hardware..." - - # - # Assume that we are PX4FMU in standalone mode - # - set BOARD PX4FMU - - # - # Are we attached to a PX4IOAR (AR.Drone carrier board)? - # - if boardinfo test name PX4IOAR - then - set BOARD PX4IOAR - if [ -f /etc/init.d/rc.PX4IOAR ] - then - echo "[init] reading /etc/init.d/rc.PX4IOAR" - usleep 500 - sh /etc/init.d/rc.PX4IOAR - fi - else - echo "[init] PX4IOAR not detected" - fi - - # - # Are we attached to a PX4IO? - # - if boardinfo test name PX4IO - then - set BOARD PX4IO - if [ -f /etc/init.d/rc.PX4IO ] - then - echo "[init] reading /etc/init.d/rc.PX4IO" - usleep 500 - sh /etc/init.d/rc.PX4IO - fi - else - echo "[init] PX4IO not detected" - fi - - # - # Looks like we are stand-alone - # - if [ $BOARD == PX4FMU ] - then - echo "[init] no expansion board detected" - if [ -f /etc/init.d/rc.standalone ] - then - echo "[init] reading /etc/init.d/rc.standalone" - sh /etc/init.d/rc.standalone - fi - fi - - # - # We may not reach here if the airframe-specific script exits the shell. - # - echo "[init] startup done." -fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl new file mode 100644 index 000000000..4686382ca --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -0,0 +1,120 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start the Mikrokopter ESC driver +# +if [ $MKBLCTRL_MODE == yes ] +then + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" + mkblctrl -mkmode x + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" + mkblctrl -mkmode + + fi +else + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" + fi + mkblctrl +fi + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io +else + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + fmu mode_pwm + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +if [ $MKBLCTRL_FRAME == x ] +then + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +fi + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc new file mode 100644 index 000000000..045e41e52 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -0,0 +1,120 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + param set MPC_XY_P 0.5 + param set MPC_XY_VEL_D 0 + param set MPC_XY_VEL_I 0 + param set MPC_XY_VEL_MAX 3 + param set MPC_XY_VEL_P 0.2 + param set MPC_Z_D 0 + param set MPC_Z_P 1 + param set MPC_Z_VEL_D 0 + param set MPC_Z_VEL_I 0.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start -d /dev/ttyS1 -b 57600 + usleep 5000 + + sh /etc/init.d/rc.io +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS1 -b 57600 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +if [ $ESC_MAKER = afro ] +then + # Set PWM values for Afro ESCs + pwm disarmed -c 1234 -p 1050 + pwm min -c 1234 -p 1080 + pwm max -c 1234 -p 1860 +else + # Set PWM values for typical ESCs + pwm disarmed -c 1234 -p 900 + pwm min -c 1234 -p 980 + pwm max -c 1234 -p 1800 +fi + +# +# Load mixer +# +if [ $FRAME_GEOMETRY == x ] +then + echo "Frame geometry X" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + if [ $FRAME_GEOMETRY == w ] + then + echo "Frame geometry W" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + else + echo "Frame geometry +" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + fi +fi + +# +# Set PWM output frequency +# +pwm rate -r 400 -c 1234 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi + +echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing new file mode 100644 index 000000000..f02851006 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -0,0 +1,34 @@ +#!nsh +# +# Standard everything needed for fixedwing except mixer, actuator output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil deleted file mode 100644 index 3517a5bd8..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ /dev/null @@ -1,68 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] starting.." - -uorb start - -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console - -# Create a fake HIL /dev/pwm_output interface -hil mode_pwm - -# -# 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 1 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Check if we got an IO -# -if [ px4io start ] -then - echo "IO started" -else - fmu mode_serial - echo "FMU started" -end - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start the attitude estimator (depends on orb) -# -att_pos_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fixedwing_backside start - -echo "[HIL] setup done, running" - -# Exit shell to make it available to MAVLink -exit diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io new file mode 100644 index 000000000..aaf91b316 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -0,0 +1,23 @@ +# +# Start PX4IO interface (depends on orb, commander) +# +if px4io start +then + # + # Allow PX4IO to recover from midair restarts. + # this is very unlikely, but quite safe and robust. + px4io recovery + + # + # Disable px4io topic limiting + # + if [ $BOARD == fmuv1 ] + then + px4io limit 200 + else + px4io limit 400 + fi +else + # SOS + tone_alarm error +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 09c2d00d1..dc4be8055 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,5 +5,10 @@ if [ -d /fs/microsd ] then - sdlog start + if [ $BOARD == fmuv1 ] + then + sdlog2 start -r 50 -a -b 16 + else + sdlog2 start -r 200 -a -b 16 + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor new file mode 100644 index 000000000..bc550ac5a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -0,0 +1,39 @@ +#!nsh +# +# Standard everything needed for multirotors except mixer, actuator output and mavlink +# + +# +# Start the sensors and test them. +# +sh /etc/init.d/rc.sensors + +# +# Start logging (depends on sensors) +# +sh /etc/init.d/rc.logging + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5e80ddc2f..f17b650bc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -7,25 +7,40 @@ # Start sensor drivers here. # -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - ms5611 start adc start +# mag might be external +if hmc5883 start +then + echo "using HMC5883" +fi + if mpu6000 start then - echo "using MPU6000 and HMC5883L" - hmc5883 start + echo "using MPU6000" + set BOARD fmuv1 else echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start + set BOARD fmuv2 +fi + +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi fi # @@ -36,8 +51,5 @@ fi # 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 5b1bd272e..ccf2cd47e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -8,14 +8,28 @@ echo "Starting MAVLink on this USB console" # Stop tone alarm tone_alarm stop +# +# Check for UORB +# +if uorb start +then + echo "uORB started" +fi + # Tell MAVLink that this link is "fast" if mavlink stop then echo "stopped other MAVLink instance" fi -sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 +# Stop commander +if commander stop +then + echo "Commander stopped" +fi +sleep 1 + # Start the commander if commander start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6..d8b5cb608 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -20,7 +20,7 @@ then else echo "[init] no microSD card found" # Play SOS - tone_alarm 2 + tone_alarm error fi # @@ -57,90 +57,301 @@ fi if [ $MODE == autostart ] then + # + # Start terminal + # + if sercon + then + echo "USB connected" + fi + + # + # Start the ORB (first app to start) + # + uorb start + + # + # Load microSD params + # + #if ramtron start + #then + # param select /ramtron/params + # if [ -f /ramtron/params ] + # then + # param load /ramtron/params + # fi + #else + param select /fs/microsd/params + if [ -f /fs/microsd/params ] + then + if param load /fs/microsd/params + then + echo "Parameters loaded" + else + echo "Parameter file corrupt - ignoring" + fi + fi + #fi + + # + # Start system state indicator + # + if rgbled start + then + echo "Using external RGB Led" + else + if blinkm start + then + blinkm systemstate + fi + fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start -# -# Start the ORB (first app to start) -# -uorb start + if param compare SYS_AUTOSTART 1000 + then + sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1002 + then + sh /etc/init.d/1002_rc_fw_state.hil + set MODE custom + fi + + if param compare SYS_AUTOSTART 1003 + then + sh /etc/init.d/1003_rc_quad_+.hil + set MODE custom + fi -# -# Load microSD params -# -if ramtron start -then - param select /ramtron/params - if [ -f /ramtron/params ] + if param compare SYS_AUTOSTART 1004 then - param load /ramtron/params + sh /etc/init.d/1004_rc_fw_Rascal110.hil + set MODE custom fi -else - param select /fs/microsd/params - if [ -f /fs/microsd/params ] + + if [ $MODE != custom ] then - param load /fs/microsd/params + # Try to get an USB console + nshterm /dev/ttyACM0 & 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 + + # + # Upgrade PX4IO firmware + # + if px4io detect then - echo "No newer version, skipping upgrade." + echo "PX4IO running, not upgrading" else - echo "Loading /fs/microsd/px4io.bin" - if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log + echo "Attempting to upgrade PX4IO" + if px4io update then - cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur - echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log + if [ -d /fs/microsd ] + then + echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + fi + + # Allow IO to safely kick back to app + usleep 200000 else - echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log - echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode" + echo "No PX4IO to upgrade here" fi fi -fi + + # + # Check if auto-setup from one of the standard scripts is wanted + # SYS_AUTOSTART = 0 means no autostart (default) + # + + if param compare SYS_AUTOSTART 8 + then + sh /etc/init.d/08_ardrone + set MODE custom + fi + + if param compare SYS_AUTOSTART 9 + then + sh /etc/init.d/09_ardrone_flow + set MODE custom + fi + + if param compare SYS_AUTOSTART 10 + then + sh /etc/init.d/10_dji_f330 + set MODE custom + fi -# -# 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 - sh /etc/init.d/01_fmu_quad_x -fi + if param compare SYS_AUTOSTART 11 + then + sh /etc/init.d/11_dji_f450 + set MODE custom + fi -if param compare SYS_AUTOSTART 2 -then - sh /etc/init.d/02_io_quad_x -fi + if param compare SYS_AUTOSTART 12 + then + set MIXER /etc/mixers/FMU_hex_x.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi -if param compare SYS_AUTOSTART 8 -then - sh /etc/init.d/08_ardrone -fi + if param compare SYS_AUTOSTART 13 + then + set MIXER /etc/mixers/FMU_hex_+.mix + sh /etc/init.d/12-13_hex + set MODE custom + fi + + if param compare SYS_AUTOSTART 15 + then + sh /etc/init.d/15_tbs_discovery + set MODE custom + fi -if param compare SYS_AUTOSTART 9 -then - sh /etc/init.d/09_ardrone_flow -fi + if param compare SYS_AUTOSTART 16 + then + sh /etc/init.d/16_3dr_iris + set MODE custom + fi -if param compare SYS_AUTOSTART 10 -then - sh /etc/init.d/10_io_f330 -fi + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame + if param compare SYS_AUTOSTART 17 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi -if param compare SYS_AUTOSTART 30 -then - sh /etc/init.d/30_io_camflyer -fi + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame + if param compare SYS_AUTOSTART 18 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi -if param compare SYS_AUTOSTART 31 -then - sh /etc/init.d/31_io_phantom -fi + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 19 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 20 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 21 + then + set FRAME_GEOMETRY x + set ESC_MAKER afro + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 22 + then + set FRAME_GEOMETRY w + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi + + if param compare SYS_AUTOSTART 30 + then + sh /etc/init.d/30_io_camflyer + set MODE custom + fi + + if param compare SYS_AUTOSTART 31 + then + sh /etc/init.d/31_io_phantom + set MODE custom + fi + + if param compare SYS_AUTOSTART 32 + then + sh /etc/init.d/32_skywalker_x5 + set MODE custom + fi + + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi + + if param compare SYS_AUTOSTART 100 + then + sh /etc/init.d/100_mpx_easystar + set MODE custom + fi + + if param compare SYS_AUTOSTART 101 + then + sh /etc/init.d/101_hk_bixler + set MODE custom + fi + + # Start any custom extensions that might be missing + if [ -f /fs/microsd/etc/rc.local ] + then + sh /fs/microsd/etc/rc.local + fi + + # If none of the autostart scripts triggered, get a minimal setup + if [ $MODE == autostart ] + then + # Telemetry port is on both FMU boards ttyS1 + # but the AR.Drone motors can be get 'flashed' + # if starting MAVLink on them - so do not + # start it as default (default link: USB) + + # Start commander + commander start + + # Start px4io if present + if px4io detect + then + px4io start + else + if fmu mode_serial + then + echo "FMU driver (no PWM) started" + fi + fi + + # Start sensors + sh /etc/init.d/rc.sensors + + # Start one of the estimators + attitude_estimator_ekf start + + # Start GPS + gps start + + fi # End of autostart fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 75e82bb00..0ec663e35 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -62,3 +62,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix index 20cb88b91..c73cb2a62 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -58,3 +58,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index ebcb66b24..17ff71151 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -50,3 +50,21 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix index 95beb8927..f07c34ac8 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -51,3 +51,23 @@ range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 9f81e1dc3..610868354 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -48,3 +48,22 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix index 981466704..f0aa6650d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -48,3 +48,23 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index b5e38ce9e..f8f9f0e4d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co are mixed 100%. R: 6+ 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 8e8d122ad..26b40b9e9 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration. All c are mixed 100%. R: 6x 10000 10000 10000 0 + +Gimbal / payload mixer for last two channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix new file mode 100644 index 000000000..51cebb785 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls +are mixed 100%. + +R: 8c 10000 10000 10000 0 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix index dfdf1d58e..cd9a9cfab 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con are mixed 100%. R: 4+ 10000 10000 10000 0 + + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix index 2a4a0f341..520aba635 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration. All co are mixed 100%. R: 4v 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix index 81b4af30b..58e6af74b 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix @@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%. R: 4w 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix index 12a3bee20..fa21c8012 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration. All co are mixed 100%. R: 4x 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone new file mode 100644 index 000000000..67e95215b --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -0,0 +1,13 @@ +#!nsh +# +# Flight startup script for PX4FMU standalone configuration. +# + +echo "[init] doing standalone PX4FMU startup..." + +# +# Start the ORB +# +uorb start + +echo "[init] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS new file mode 100755 index 000000000..7f161b053 --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -0,0 +1,4 @@ +#!nsh +# +# PX4FMU startup script for test hackery. +# diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore new file mode 100644 index 000000000..73cf39575 --- /dev/null +++ b/Tools/px4params/.gitignore @@ -0,0 +1,2 @@ +parameters.wiki +parameters.xml
\ No newline at end of file diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md new file mode 100644 index 000000000..a23b44799 --- /dev/null +++ b/Tools/px4params/README.md @@ -0,0 +1,9 @@ +h1. PX4 Parameters Processor + +It's designed to scan PX4 source codes, find declarations of tunable parameters, +and generate the list in various formats. + +Currently supported formats are: + +* XML for the parametric UI generator +* Human-readable description in DokuWiki format diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py new file mode 100644 index 000000000..33f76b415 --- /dev/null +++ b/Tools/px4params/dokuwikiout.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py new file mode 100644 index 000000000..c09246871 --- /dev/null +++ b/Tools/px4params/output.py @@ -0,0 +1,5 @@ +class Output(object): + def Save(self, groups, fn): + data = self.Generate(groups) + with open(fn, 'w') as f: + f.write(data) diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py new file mode 100644 index 000000000..251be672f --- /dev/null +++ b/Tools/px4params/parser.py @@ -0,0 +1,220 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + cmp=lambda x, y: cmp(x.GetFieldValue("code"), + y.GetFieldValue("code"))) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "code": 10, + "type": 9, + "short_desc": 8, + "long_desc": 7, + "default": 6, + "min": 5, + "max": 4, + # all others == 0 (sorted alphabetically) + } + + def __init__(self): + self.fields = {} + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + return sorted(self.fields.keys(), + cmp=lambda x, y: cmp(self.priority.get(y, 0), + self.priority.get(x, 0)) or cmp(x, y)) + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + return self.fields.get(code) + +class Parser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + + valid_tags = set(["min", "max", "group"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. + """ + return ["cpp", "c"] + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + # Non-empty line outside the comment + m = self.re_parameter_definition.match(line) + if m: + tp, code, defval = m.group(1, 2, 3) + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid" + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.param_groups.values(), + cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), + self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), + y.GetName())) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py new file mode 100755 index 000000000..cdf5aba7c --- /dev/null +++ b/Tools/px4params/px_process_params.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# It scans src/ subdirectory of the project, collects all parameters +# definitions, and outputs list of parameters in XML and DokuWiki formats. +# + +import scanner +import parser +import xmlout +import dokuwikiout + +# Initialize parser +prs = parser.Parser() + +# Scan directories, and parse the files +sc = scanner.Scanner() +sc.ScanDir("../../src", prs) +output = prs.GetParamGroups() + +# Output into XML +out = xmlout.XMLOutput() +out.Save(output, "parameters.xml") + +# Output into DokuWiki +out = dokuwikiout.DokuWikiOutput() +out.Save(output, "parameters.wiki") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py new file mode 100644 index 000000000..b5a1af47c --- /dev/null +++ b/Tools/px4params/scanner.py @@ -0,0 +1,34 @@ +import os +import re + +class Scanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + re_file_extension = re.compile(r'\.([^\.]+)$') + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = set(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + m = self.re_file_extension.search(filename) + if m: + ext = m.group(1) + if ext in extensions: + path = os.path.join(dirname, filename) + self.ScanFile(path, parser) + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with open(path, 'r') as f: + contents = f.read() + parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py new file mode 100644 index 000000000..d56802b15 --- /dev/null +++ b/Tools/px4params/xmlout.py @@ -0,0 +1,22 @@ +import output +from xml.dom.minidom import getDOMImplementation + +class XMLOutput(output.Output): + def Generate(self, groups): + impl = getDOMImplementation() + xml_document = impl.createDocument(None, "parameters", None) + xml_parameters = xml_document.documentElement + for group in groups: + xml_group = xml_document.createElement("group") + xml_group.setAttribute("name", group.GetName()) + xml_parameters.appendChild(xml_group) + for param in group.GetParams(): + xml_param = xml_document.createElement("parameter") + xml_group.appendChild(xml_param) + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + xml_field = xml_document.createElement(code) + xml_param.appendChild(xml_field) + xml_value = xml_document.createTextNode(value) + xml_field.appendChild(xml_value) + return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index faef9ca33..b598a65a1 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# 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 @@ -60,7 +60,7 @@ def mkdesc(): proto['description'] = "" proto['git_identity'] = "" proto['build_time'] = 0 - proto['image'] = base64.b64encode(bytearray()) + proto['image'] = bytes() proto['image_size'] = 0 return proto @@ -99,12 +99,12 @@ if args.description != None: if args.git_identity != None: cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"]) p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout - desc['git_identity'] = p.read().strip() + desc['git_identity'] = str(p.read().strip()) p.close() if args.image != None: f = open(args.image, "rb") bytes = f.read() desc['image_size'] = len(bytes) - desc['image'] = base64.b64encode(zlib.compress(bytes,9)) + desc['image'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') print(json.dumps(desc, indent=4)) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d2ebf1698..a2d7d371d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# 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 @@ -102,7 +102,7 @@ class firmware(object): 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) - crcpad = bytearray('\xff\xff\xff\xff') + crcpad = bytearray(b'\xff\xff\xff\xff') def __init__(self, path): @@ -137,34 +137,40 @@ class uploader(object): '''Uploads a firmware file to the PX FMU bootloader''' # protocol bytes - INSYNC = chr(0x12) - EOC = chr(0x20) + INSYNC = b'\x12' + EOC = b'\x20' # reply bytes - OK = chr(0x10) - FAILED = chr(0x11) - INVALID = chr(0x13) # rev3+ + OK = b'\x10' + FAILED = b'\x11' + INVALID = b'\x13' # rev3+ # command bytes - NOP = chr(0x00) # guaranteed to be discarded by the bootloader - GET_SYNC = chr(0x21) - GET_DEVICE = chr(0x22) - CHIP_ERASE = chr(0x23) - CHIP_VERIFY = chr(0x24) # rev2 only - PROG_MULTI = chr(0x27) - READ_MULTI = chr(0x28) # rev2 only - GET_CRC = chr(0x29) # rev3+ - REBOOT = chr(0x30) + NOP = b'\x00' # guaranteed to be discarded by the bootloader + GET_SYNC = b'\x21' + GET_DEVICE = b'\x22' + CHIP_ERASE = b'\x23' + CHIP_VERIFY = b'\x24' # rev2 only + PROG_MULTI = b'\x27' + READ_MULTI = b'\x28' # rev2 only + GET_CRC = b'\x29' # rev3+ + REBOOT = b'\x30' - INFO_BL_REV = chr(1) # bootloader protocol revision + INFO_BL_REV = b'\x01' # bootloader protocol revision BL_REV_MIN = 2 # minimum supported bootloader protocol - BL_REV_MAX = 3 # maximum supported bootloader protocol - INFO_BOARD_ID = chr(2) # board type - INFO_BOARD_REV = chr(3) # board revision - INFO_FLASH_SIZE = chr(4) # max firmware size in bytes + BL_REV_MAX = 4 # maximum supported bootloader protocol + INFO_BOARD_ID = b'\x02' # board type + INFO_BOARD_REV = b'\x03' # board revision + INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64 + + NSH_INIT = bytearray(b'\x0d\x0d\x0d') + NSH_REBOOT_BL = b"reboot -b\n" + NSH_REBOOT = b"reboot\n" + MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0') + MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac') def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly @@ -176,7 +182,7 @@ class uploader(object): def __send(self, c): # print("send " + binascii.hexlify(c)) - self.port.write(str(c)) + self.port.write(c) def __recv(self, count=1): c = self.port.read(count) @@ -192,9 +198,9 @@ class uploader(object): def __getSync(self): self.port.flush() - c = self.__recv() - if c is not self.INSYNC: - raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c)) + c = bytes(self.__recv()) + if c != self.INSYNC: + raise RuntimeError("unexpected %s instead of INSYNC" % c) c = self.__recv() if c == self.INVALID: raise RuntimeError("bootloader reports INVALID OPERATION") @@ -249,17 +255,29 @@ class uploader(object): # send a PROG_MULTI command to write a collection of bytes def __program_multi(self, data): - self.__send(uploader.PROG_MULTI - + chr(len(data))) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.PROG_MULTI) + self.__send(length) self.__send(data) self.__send(uploader.EOC) self.__getSync() # verify multiple bytes in flash def __verify_multi(self, data): - self.__send(uploader.READ_MULTI - + chr(len(data)) - + uploader.EOC) + + if runningPython3 == True: + length = len(data).to_bytes(1, byteorder='big') + else: + length = chr(len(data)) + + self.__send(uploader.READ_MULTI) + self.__send(length) + self.__send(uploader.EOC) self.port.flush() programmed = self.__recv(len(data)) if programmed != data: @@ -350,7 +368,24 @@ class uploader(object): print("done, rebooting.") self.__reboot() self.port.close() - + + def send_reboot(self): + # try reboot via NSH first + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT_BL) + self.__send(uploader.NSH_INIT) + self.__send(uploader.NSH_REBOOT) + # then try MAVLINK command + self.__send(uploader.MAVLINK_REBOOT_ID1) + self.__send(uploader.MAVLINK_REBOOT_ID0) + + + +# Detect python version +if sys.version_info[0] < 3: + runningPython3 = False +else: + runningPython3 = True # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") @@ -365,7 +400,19 @@ print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property( # Spin waiting for a device to show up while True: - for port in args.port.split(","): + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: #print("Trying %s" % port) @@ -383,7 +430,7 @@ while True: # Windows, don't open POSIX ports if not "/" in port: up = uploader(port, args.baud) - except: + except Exception: # open failed, rate-limit our attempts time.sleep(0.05) @@ -396,8 +443,12 @@ while True: up.identify() print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except: - # most probably a timeout talking to the port, no bootloader + except Exception: + # most probably a timeout talking to the port, no bootloader, try to reboot the board + print("attempting reboot on %s..." % port) + up.send_reboot() + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.5) continue try: diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 7fefc5908..5b1e55e22 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -25,8 +25,12 @@ import struct, sys if sys.hexversion >= 0x030000F0: runningPython3 = True + def _parseCString(cstr): + return str(cstr, 'ascii').split('\0')[0] else: runningPython3 = False + def _parseCString(cstr): + return str(cstr).split('\0')[0] class SDLog2Parser: BLOCK_SIZE = 8192 @@ -175,9 +179,9 @@ class SDLog2Parser: self.__csv_columns.append(full_label) self.__csv_data[full_label] = None if self.__file != None: - print(self.__csv_delim.join(self.__csv_columns), file=self.__file) + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) else: - print(self.__csv_delim.join(self.__csv_columns)) + print(self.__csv_delim.join(self.__csv_columns)) def __printCSVRow(self): s = [] @@ -190,9 +194,9 @@ class SDLog2Parser: s.append(v) if self.__file != None: - print(self.__csv_delim.join(s), file=self.__file) + print(self.__csv_delim.join(s), file=self.__file) else: - print(self.__csv_delim.join(s)) + print(self.__csv_delim.join(s)) def __parseMsgDescr(self): if runningPython3: @@ -202,14 +206,9 @@ class SDLog2Parser: msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - 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(",") + msg_name = _parseCString(data[2]) + msg_format = _parseCString(data[3]) + msg_labels = _parseCString(data[4]).split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -243,7 +242,7 @@ class SDLog2Parser: 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") + data[i] = _parseCString(data[i]) m = msg_mults[i] if m != None: data[i] = data[i] * m diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore new file mode 100644 index 000000000..61e091551 --- /dev/null +++ b/Tools/tests-host/.gitignore @@ -0,0 +1,2 @@ +./obj/* +mixer_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile new file mode 100644 index 000000000..97410ff47 --- /dev/null +++ b/Tools/tests-host/Makefile @@ -0,0 +1,39 @@ + +CC=g++ +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" + +ODIR=obj +LDIR =../lib + +LIBS=-lm + +#_DEPS = test.h +#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) + +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) + +#$(DEPS) +$(ODIR)/%.o: %.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c + $(CC) -c -o $@ $< $(CFLAGS) + +# +mixer_test: $(OBJ) + g++ -o $@ $^ $(CFLAGS) $(LIBS) + +.PHONY: clean + +clean: + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/Tools/tests-host/arch/board/board.h diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp new file mode 100644 index 000000000..042322aad --- /dev/null +++ b/Tools/tests-host/mixer_test.cpp @@ -0,0 +1,12 @@ +#include <systemlib/mixer/mixer.h> +#include <systemlib/err.h> +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("Host execution started"); + + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + + test_mixer(3, args); +}
\ No newline at end of file diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/Tools/tests-host/nuttx/config.h diff --git a/makefiles/board_px4fmu-v2.mk b/makefiles/board_px4fmu-v2.mk new file mode 100644 index 000000000..e9a2985b7 --- /dev/null +++ b/makefiles/board_px4fmu-v2.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the PX4FMUv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = PX4FMU_V2 + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/board_px4io-v2.mk b/makefiles/board_px4io-v2.mk new file mode 100644 index 000000000..50a4068fb --- /dev/null +++ b/makefiles/board_px4io-v2.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the PX4IOv2 +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM3 +CONFIG_BOARD = PX4IO_V2 + +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 27543e8bb..306827086 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -6,6 +6,7 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin # # Board support modules @@ -17,7 +18,7 @@ MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/px4io MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu +MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 MODULES += drivers/bma180 @@ -30,8 +31,9 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/md25 +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed @@ -50,15 +52,18 @@ MODULES += systemcmds/param MODULES += systemcmds/perf MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control # MODULES += modules/commander +MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard MODULES += modules/gpio_led @@ -67,21 +72,17 @@ MODULES += modules/gpio_led # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp -MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway -MODULES += modules/fixedwing_backside -MODULES += modules/fixedwing_att_control -MODULES += modules/fixedwing_pos_control -MODULES += modules/fw_att_control_vector +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control -MODULES += modules/mc_att_control_vector MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control MODULES += examples/flow_speed_control @@ -92,19 +93,29 @@ MODULES += examples/flow_speed_control MODULES += modules/sdlog2 # +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# # Library modules # MODULES += modules/systemlib MODULES += modules/systemlib/mixer -MODULES += modules/mathlib -MODULES += modules/mathlib/math/filter MODULES += modules/controllib MODULES += modules/uORB # # Libraries # -LIBRARIES += modules/mathlib/CMSIS +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion # # Demo apps @@ -124,7 +135,10 @@ LIBRARIES += modules/mathlib/CMSIS # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk new file mode 100644 index 000000000..761fb8d9d --- /dev/null +++ b/makefiles/config_px4fmu-v2_default.mk @@ -0,0 +1,155 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + +# +# System commands +# +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard + +# +# Estimation modules (EKF / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk new file mode 100644 index 000000000..0f60e88b5 --- /dev/null +++ b/makefiles/config_px4fmu-v2_test.mk @@ -0,0 +1,41 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/led +MODULES += drivers/boards/px4fmu-v2 +MODULES += systemcmds/perf +MODULES += systemcmds/reboot + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/uORB + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4io-v1_default.mk b/makefiles/config_px4io-v1_default.mk index cf70391bc..73f8adf20 100644 --- a/makefiles/config_px4io-v1_default.mk +++ b/makefiles/config_px4io-v1_default.mk @@ -6,5 +6,5 @@ # Board support modules # MODULES += drivers/stm32 -MODULES += drivers/boards/px4io +MODULES += drivers/boards/px4io-v1 MODULES += modules/px4iofirmware
\ No newline at end of file diff --git a/makefiles/config_px4io-v2_default.mk b/makefiles/config_px4io-v2_default.mk new file mode 100644 index 000000000..dbeaba3d3 --- /dev/null +++ b/makefiles/config_px4io-v2_default.mk @@ -0,0 +1,10 @@ +# +# Makefile for the px4iov2_default configuration +# + +# +# Board support modules +# +MODULES += drivers/stm32 +MODULES += drivers/boards/px4io-v2 +MODULES += modules/px4iofirmware
\ No newline at end of file diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index ecff77db9..cb20d9cd1 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -110,6 +110,8 @@ ifneq ($(words $(PX4_BASE)),1) $(error Cannot build when the PX4_BASE path contains one or more space characters.) endif +$(info % GIT_DESC = $(GIT_DESC)) + # # Set a default target so that included makefiles or errors here don't # cause confusion. @@ -177,6 +179,17 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = + +# +# Extra defines for compilation +# +export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC) + +# +# Append the per-board driver directory to the header search path. +# +INCLUDE_DIRS += $(PX4_MODULE_SRC)drivers/boards/$(BOARD) + ################################################################################ # NuttX libraries and paths ################################################################################ @@ -317,7 +330,7 @@ endif # a root from several templates. That would be a nice feature. # -# Add dependencies on anything in the ROMFS root +# Add dependencies on anything in the ROMFS root directory ROMFS_FILES += $(wildcard \ $(ROMFS_ROOT)/* \ $(ROMFS_ROOT)/*/* \ @@ -329,7 +342,14 @@ ifeq ($(ROMFS_FILES),) $(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files) endif ROMFS_DEPS += $(ROMFS_FILES) + +# Extra files that may be copied into the ROMFS /extras directory +# ROMFS_EXTRA_FILES are required, ROMFS_OPTIONAL_FILES are optional +ROMFS_EXTRA_FILES += $(wildcard $(ROMFS_OPTIONAL_FILES)) +ROMFS_DEPS += $(ROMFS_EXTRA_FILES) + ROMFS_IMG = romfs.img +ROMFS_SCRATCH = romfs_scratch ROMFS_CSRC = $(ROMFS_IMG:.img=.c) ROMFS_OBJ = $(ROMFS_CSRC:.c=.o) LIBS += $(ROMFS_OBJ) @@ -340,9 +360,18 @@ $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS) $(call BIN_TO_OBJ,$<,$@,romfs_img) # Generate the ROMFS image from the root -$(ROMFS_IMG): $(ROMFS_DEPS) $(GLOBAL_DEPS) +$(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) @$(ECHO) "ROMFS: $@" - $(Q) $(GENROMFS) -f $@ -d $(ROMFS_ROOT) -V "NSHInitVol" + $(Q) $(GENROMFS) -f $@ -d $(ROMFS_SCRATCH) -V "NSHInitVol" + +# Construct the ROMFS scratch root from the canonical root +$(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) + $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +ifneq ($(ROMFS_EXTRA_FILES),) + $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras + $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras +endif EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5c..183b143d6 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -43,6 +43,7 @@ # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ +export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ @@ -57,7 +58,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ # export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ - $(PX4_INCLUDE_DIR) + $(PX4_INCLUDE_DIR) \ + $(PX4_LIB_DIR) # # Tools @@ -71,6 +73,7 @@ export RMDIR = rm -rf export GENROMFS = genromfs export TOUCH = touch export MKDIR = mkdir +export FIND = find export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 3aebef863..bc26d743d 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -12,13 +12,13 @@ SYSTYPE := $(shell uname -s) # XXX The uploader should be smarter than this. # ifeq ($(SYSTYPE),Darwin) -SERIAL_PORTS ?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" +SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) -SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM28,\\\\.\\COM27,\\\\.\\COM26,\\\\.\\COM25,\\\\.\\COM24,\\\\.\\COM23,\\\\.\\COM22,\\\\.\\COM21,\\\\.\\COM20,\\\\.\\COM19,\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0" +SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" endif .PHONY: all upload-$(METHOD)-$(BOARD) @@ -27,6 +27,8 @@ all: upload-$(METHOD)-$(BOARD) upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) +upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) + $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) # # JTAG firmware uploading with OpenOCD diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index eec9a89c5..3aad8e678 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -74,6 +74,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ @@ -86,7 +87,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif @@ -179,6 +181,7 @@ enum LIMIT_MODULE #include "./mavlink_msg_data64.h" #include "./mavlink_msg_data96.h" #include "./mavlink_msg_rangefinder.h" +#include "./mavlink_msg_airspeed_autocal.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h index d9d2ccb04..c3ead1140 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen * @brief Pack a ahrs message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param omegaIx X gyro drift estimate rad/s * @param omegaIy Y gyro drift estimate rad/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ahrs struct into a message + * @brief Encode a ahrs struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a ahrs struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** * @brief Send a ahrs message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h new file mode 100644 index 000000000..d046f2ad0 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -0,0 +1,419 @@ +// MESSAGE AIRSPEED_AUTOCAL PACKING + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 + +typedef struct __mavlink_airspeed_autocal_t +{ + float vx; ///< GPS velocity north m/s + float vy; ///< GPS velocity east m/s + float vz; ///< GPS velocity down m/s + float diff_pressure; ///< Differential pressure pascals + float EAS2TAS; ///< Estimated to true airspeed ratio + float ratio; ///< Airspeed ratio + float state_x; ///< EKF state x + float state_y; ///< EKF state y + float state_z; ///< EKF state z + float Pax; ///< EKF Pax + float Pby; ///< EKF Pby + float Pcz; ///< EKF Pcz +} mavlink_airspeed_autocal_t; + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 +#define MAVLINK_MSG_ID_174_LEN 48 + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 +#define MAVLINK_MSG_ID_174_CRC 167 + + + +#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ + "AIRSPEED_AUTOCAL", \ + 12, \ + { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ + { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ + { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ + { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ + { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ + { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ + { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ + { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ + { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ + } \ +} + + +/** + * @brief Pack a airspeed_autocal message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Pack a airspeed_autocal message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Encode a airspeed_autocal struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Encode a airspeed_autocal struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Send a airspeed_autocal message + * @param chan MAVLink channel to send the message + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AIRSPEED_AUTOCAL UNPACKING + + +/** + * @brief Get field vx from airspeed_autocal message + * + * @return GPS velocity north m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field vy from airspeed_autocal message + * + * @return GPS velocity east m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field vz from airspeed_autocal message + * + * @return GPS velocity down m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field diff_pressure from airspeed_autocal message + * + * @return Differential pressure pascals + */ +static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field EAS2TAS from airspeed_autocal message + * + * @return Estimated to true airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field ratio from airspeed_autocal message + * + * @return Airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field state_x from airspeed_autocal message + * + * @return EKF state x + */ +static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field state_y from airspeed_autocal message + * + * @return EKF state y + */ +static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field state_z from airspeed_autocal message + * + * @return EKF state z + */ +static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field Pax from airspeed_autocal message + * + * @return EKF Pax + */ +static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field Pby from airspeed_autocal message + * + * @return EKF Pby + */ +static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field Pcz from airspeed_autocal message + * + * @return EKF Pcz + */ +static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a airspeed_autocal message into a struct + * + * @param msg The message to decode + * @param airspeed_autocal C-struct to decode the message contents into + */ +static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) +{ +#if MAVLINK_NEED_BYTE_SWAP + airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); + airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); + airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); + airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); + airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); + airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); + airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); + airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); + airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); + airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); + airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); + airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); +#else + memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h index c6c762451..821ce73e4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon * @brief Pack a ap_adc message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC output 1 * @param adc2 ADC output 2 @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a ap_adc struct into a message + * @brief Encode a ap_adc struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a ap_adc struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** * @brief Send a ap_adc message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h index c75358526..9200eefa0 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t compon * @brief Pack a data16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data16 struct into a message + * @brief Encode a data16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) +{ + return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); +} + +/** * @brief Send a data16 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h index 46c804a3c..3afedb787 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t compon * @brief Pack a data32 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data32 struct into a message + * @brief Encode a data32 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data32 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data32 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) +{ + return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); +} + +/** * @brief Send a data32 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h index 843084dff..6931ada16 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t compon * @brief Pack a data64 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data64 struct into a message + * @brief Encode a data64 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data64 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data64 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) +{ + return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); +} + +/** * @brief Send a data64 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h index 095628865..cffc7d7e7 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t compon * @brief Pack a data96 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data96 struct into a message + * @brief Encode a data96 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data96 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data96 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) +{ + return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); +} + +/** * @brief Send a data96 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h index bcc706a88..c6518c419 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin * @brief Pack a digicam_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id } /** - * @brief Encode a digicam_configure struct into a message + * @brief Encode a digicam_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, u } /** + * @brief Encode a digicam_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) +{ + return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); +} + +/** * @brief Send a digicam_configure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h index 7fa8cdfef..bfa5414a3 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8 * @brief Pack a digicam_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a digicam_control struct into a message + * @brief Encode a digicam_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uin } /** + * @brief Encode a digicam_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) +{ + return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); +} + +/** * @brief Send a digicam_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h index 2cd4fc798..fe3677d53 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uin * @brief Pack a fence_fetch_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id } /** - * @brief Encode a fence_fetch_point struct into a message + * @brief Encode a fence_fetch_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, u } /** + * @brief Encode a fence_fetch_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) +{ + return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); +} + +/** * @brief Send a fence_fetch_point message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h index b3c4706ee..febda6cdc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c * @brief Pack a fence_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a fence_point struct into a message + * @brief Encode a fence_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a fence_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) +{ + return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); +} + +/** * @brief Send a fence_point message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h index 32f2bc03a..612090406 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t * @brief Pack a fence_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param breach_status 0 if currently inside fence, 1 if outside * @param breach_count number of fence breaches @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a fence_status struct into a message + * @brief Encode a fence_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a fence_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); +} + +/** * @brief Send a fence_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h index 73870ec0f..2f5dea513 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t comp * @brief Pack a hwstatus message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Vcc board voltage (mV) * @param I2Cerr I2C error count @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hwstatus struct into a message + * @brief Encode a hwstatus struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a hwstatus struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hwstatus C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) +{ + return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); +} + +/** * @brief Send a hwstatus message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h index f7b04fba7..34743fd02 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t * @brief Pack a limits_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) * @param last_trigger time of last breach in milliseconds since boot @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a limits_status struct into a message + * @brief Encode a limits_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8 } /** + * @brief Encode a limits_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** * @brief Send a limits_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h index 437029eed..55f772bbc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t compo * @brief Pack a meminfo message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param brkval heap top * @param freemem free memory @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a meminfo struct into a message + * @brief Encode a meminfo struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a meminfo struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param meminfo C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) +{ + return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); +} + +/** * @brief Send a meminfo message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h index 450153c6f..de717dfa4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8 * @brief Pack a mount_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, } /** - * @brief Encode a mount_configure struct into a message + * @brief Encode a mount_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uin } /** + * @brief Encode a mount_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) +{ + return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); +} + +/** * @brief Send a mount_configure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h index 5b83d7e97..44416353e 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t * @brief Pack a mount_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mount_control struct into a message + * @brief Encode a mount_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8 } /** + * @brief Encode a mount_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) +{ + return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); +} + +/** * @brief Send a mount_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h index c031db42b..4905905dc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t * @brief Pack a mount_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mount_status struct into a message + * @brief Encode a mount_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a mount_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) +{ + return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); +} + +/** * @brief Send a mount_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h index e13776993..8e9740e82 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone * @brief Pack a radio message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a radio struct into a message + * @brief Encode a radio struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo } /** + * @brief Encode a radio struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); +} + +/** * @brief Send a radio message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h index d88abe36a..c476447a8 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t c * @brief Pack a rangefinder message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param distance distance in meters * @param voltage raw voltage if available, zero otherwise @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a rangefinder struct into a message + * @brief Encode a rangefinder struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a rangefinder struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rangefinder C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) +{ + return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); +} + +/** * @brief Send a rangefinder message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h index d121e48e6..31b7d989d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_ * @brief Pack a sensor_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param mag_ofs_x magnetometer X offset * @param mag_ofs_y magnetometer Y offset @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u } /** - * @brief Encode a sensor_offsets struct into a message + * @brief Encode a sensor_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -190,6 +190,20 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint } /** + * @brief Encode a sensor_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + +/** * @brief Send a sensor_offsets message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h index a59e90649..b2c629c39 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8 * @brief Pack a set_mag_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_mag_offsets struct into a message + * @brief Encode a set_mag_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin } /** + * @brief Encode a set_mag_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + +/** * @brief Send a set_mag_offsets message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h index 7373c8bff..8ef44f76d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h @@ -13,15 +13,15 @@ typedef struct __mavlink_simstate_t float xgyro; ///< Angular speed around X axis rad/s float ygyro; ///< Angular speed around Y axis rad/s float zgyro; ///< Angular speed around Z axis rad/s - float lat; ///< Latitude in degrees - float lng; ///< Longitude in degrees + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 } mavlink_simstate_t; #define MAVLINK_MSG_ID_SIMSTATE_LEN 44 #define MAVLINK_MSG_ID_164_LEN 44 -#define MAVLINK_MSG_ID_SIMSTATE_CRC 111 -#define MAVLINK_MSG_ID_164_CRC 111 +#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 +#define MAVLINK_MSG_ID_164_CRC 154 @@ -37,8 +37,8 @@ typedef struct __mavlink_simstate_t { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ } \ } @@ -58,12 +58,12 @@ typedef struct __mavlink_simstate_t * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) + float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -76,8 +76,8 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @brief Pack a simstate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) @@ -120,13 +120,13 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lng) + float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -139,8 +139,8 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a simstate struct into a message + * @brief Encode a simstate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a simstate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param simstate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) +{ + return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); +} + +/** * @brief Send a simstate message * @param chan MAVLink channel to send the message * @@ -194,12 +208,12 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) +static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -212,8 +226,8 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); @@ -340,21 +354,21 @@ static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) /** * @brief Get field lat from simstate message * - * @return Latitude in degrees + * @return Latitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 36); + return _MAV_RETURN_int32_t(msg, 36); } /** * @brief Get field lng from simstate message * - * @return Longitude in degrees + * @return Longitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 40); + return _MAV_RETURN_int32_t(msg, 40); } /** diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h index ebdd2949d..7608a7bd1 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t componen * @brief Pack a wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param direction wind direction that wind is coming from (degrees) * @param speed wind speed in ground plane (m/s) @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a wind struct into a message + * @brief Encode a wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) +{ + return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); +} + +/** * @brief Send a wind message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 07bf19ee0..2c9cfef3d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -738,8 +738,8 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli 185.0, 213.0, 241.0, - 269.0, - 297.0, + 963499336, + 963499544, }; mavlink_simstate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1225,6 +1225,71 @@ static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_airspeed_autocal_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + 269.0, + 297.0, + 325.0, + }; + mavlink_airspeed_autocal_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.diff_pressure = packet_in.diff_pressure; + packet1.EAS2TAS = packet_in.EAS2TAS; + packet1.ratio = packet_in.ratio; + packet1.state_x = packet_in.state_x; + packet1.state_y = packet_in.state_y; + packet1.state_z = packet_in.state_z; + packet1.Pax = packet_in.Pax; + packet1.Pby = packet_in.Pby; + packet1.Pcz = packet_in.Pcz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack(system_id, component_id, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_airspeed_autocal_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_send(MAVLINK_COMM_1 , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_test_sensor_offsets(system_id, component_id, last_msg); @@ -1250,6 +1315,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_test_data64(system_id, component_id, last_msg); mavlink_test_data96(system_id, component_id, last_msg); mavlink_test_rangefinder(system_id, component_id, last_msg); + mavlink_test_airspeed_autocal(system_id, component_id, last_msg); } #ifdef __cplusplus diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h index 852b94b55..1b285ff7b 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:17 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:25 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index cc020b9d2..216c7805d 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -30,6 +30,30 @@ extern "C" { // ENUM DEFINITIONS +/** @brief Available operating modes/statuses for AutoQuad flight controller. + Bitmask up to 32 bits. Low side bits for base modes, high side for + additional active features/modifiers/constraints. */ +#ifndef HAVE_ENUM_AUTOQUAD_NAV_STATUS +#define HAVE_ENUM_AUTOQUAD_NAV_STATUS +enum AUTOQUAD_NAV_STATUS +{ + AQ_NAV_STATUS_INIT=0, /* System is initializing | */ + AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ + AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ + AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ + AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ + AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ + AQ_NAV_STATUS_CEILING=134217728, /* Ceiling altitude is set | */ + AQ_NAV_STATUS_HF_DYNAMIC=268435456, /* Heading-Free dynamic mode active | */ + AQ_NAV_STATUS_HF_LOCKED=536870912, /* Heading-Free locked mode active | */ + AQ_NAV_STATUS_RTH=1073741824, /* Automatic Return to Home is active | */ + AQ_NAV_STATUS_FAILSAFE=2147483648, /* System is in failsafe recovery mode | */ + AUTOQUAD_NAV_STATUS_ENUM_END=2147483649, /* | */ +}; +#endif + /** @brief */ #ifndef HAVE_ENUM_MAV_CMD #define HAVE_ENUM_MAV_CMD @@ -63,6 +87,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -71,7 +96,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h index dabccdf73..ed7c86bcb 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_ * @brief Pack a aq_telemetry_f message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Index Index of message * @param value1 value1 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, u } /** - * @brief Encode a aq_telemetry_f struct into a message + * @brief Encode a aq_telemetry_f struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -262,6 +262,20 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint } /** + * @brief Encode a aq_telemetry_f struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param aq_telemetry_f C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ + return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); +} + +/** * @brief Send a aq_telemetry_f message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index a26ce2e90..d8ba74f03 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:37 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:36 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index e2ab66ed1..9d3db3326 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -260,6 +260,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -268,7 +269,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h deleted file mode 100644 index 0c4ab8c7d..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE 6DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_6DOF_SETPOINT 149 - -typedef struct __mavlink_6dof_setpoint_t -{ - float trans_x; ///< Translational Component in x - float trans_y; ///< Translational Component in y - float trans_z; ///< Translational Component in z - float rot_x; ///< Rotational Component in x - float rot_y; ///< Rotational Component in y - float rot_z; ///< Rotational Component in z - uint8_t target_system; ///< System ID -} mavlink_6dof_setpoint_t; - -#define MAVLINK_MSG_ID_6DOF_SETPOINT_LEN 25 -#define MAVLINK_MSG_ID_149_LEN 25 - - - -#define MAVLINK_MESSAGE_INFO_6DOF_SETPOINT { \ - "6DOF_SETPOINT", \ - 7, \ - { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_6dof_setpoint_t, trans_x) }, \ - { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_6dof_setpoint_t, trans_y) }, \ - { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_6dof_setpoint_t, trans_z) }, \ - { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_6dof_setpoint_t, rot_x) }, \ - { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_6dof_setpoint_t, rot_y) }, \ - { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_6dof_setpoint_t, rot_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_6dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 6dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 25, 144); -} - -/** - * @brief Pack a 6dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 144); -} - -/** - * @brief Encode a 6dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 6dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_6dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_6dof_setpoint_t* 6dof_setpoint) -{ - return mavlink_msg_6dof_setpoint_pack(system_id, component_id, msg, 6dof_setpoint->target_system, 6dof_setpoint->trans_x, 6dof_setpoint->trans_y, 6dof_setpoint->trans_z, 6dof_setpoint->rot_x, 6dof_setpoint->rot_y, 6dof_setpoint->rot_z); -} - -/** - * @brief Send a 6dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_6dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, buf, 25, 144); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, (const char *)&packet, 25, 144); -#endif -} - -#endif - -// MESSAGE 6DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 6dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_6dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field trans_x from 6dof_setpoint message - * - * @return Translational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field trans_y from 6dof_setpoint message - * - * @return Translational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field trans_z from 6dof_setpoint message - * - * @return Translational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rot_x from 6dof_setpoint message - * - * @return Rotational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rot_y from 6dof_setpoint message - * - * @return Rotational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rot_z from 6dof_setpoint message - * - * @return Rotational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a 6dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 6dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_6dof_setpoint_decode(const mavlink_message_t* msg, mavlink_6dof_setpoint_t* 6dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 6dof_setpoint->trans_x = mavlink_msg_6dof_setpoint_get_trans_x(msg); - 6dof_setpoint->trans_y = mavlink_msg_6dof_setpoint_get_trans_y(msg); - 6dof_setpoint->trans_z = mavlink_msg_6dof_setpoint_get_trans_z(msg); - 6dof_setpoint->rot_x = mavlink_msg_6dof_setpoint_get_rot_x(msg); - 6dof_setpoint->rot_y = mavlink_msg_6dof_setpoint_get_rot_y(msg); - 6dof_setpoint->rot_z = mavlink_msg_6dof_setpoint_get_rot_z(msg); - 6dof_setpoint->target_system = mavlink_msg_6dof_setpoint_get_target_system(msg); -#else - memcpy(6dof_setpoint, _MAV_PAYLOAD(msg), 25); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h deleted file mode 100644 index 521d2fb66..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h +++ /dev/null @@ -1,320 +0,0 @@ -// MESSAGE 8DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_8DOF_SETPOINT 148 - -typedef struct __mavlink_8dof_setpoint_t -{ - float val1; ///< Value 1 - float val2; ///< Value 2 - float val3; ///< Value 3 - float val4; ///< Value 4 - float val5; ///< Value 5 - float val6; ///< Value 6 - float val7; ///< Value 7 - float val8; ///< Value 8 - uint8_t target_system; ///< System ID -} mavlink_8dof_setpoint_t; - -#define MAVLINK_MSG_ID_8DOF_SETPOINT_LEN 33 -#define MAVLINK_MSG_ID_148_LEN 33 - - - -#define MAVLINK_MESSAGE_INFO_8DOF_SETPOINT { \ - "8DOF_SETPOINT", \ - 9, \ - { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_8dof_setpoint_t, val1) }, \ - { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_8dof_setpoint_t, val2) }, \ - { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_8dof_setpoint_t, val3) }, \ - { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_8dof_setpoint_t, val4) }, \ - { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_8dof_setpoint_t, val5) }, \ - { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_8dof_setpoint_t, val6) }, \ - { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_8dof_setpoint_t, val7) }, \ - { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_8dof_setpoint_t, val8) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_8dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 8dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 33, 42); -} - -/** - * @brief Pack a 8dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 42); -} - -/** - * @brief Encode a 8dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 8dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_8dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_8dof_setpoint_t* 8dof_setpoint) -{ - return mavlink_msg_8dof_setpoint_pack(system_id, component_id, msg, 8dof_setpoint->target_system, 8dof_setpoint->val1, 8dof_setpoint->val2, 8dof_setpoint->val3, 8dof_setpoint->val4, 8dof_setpoint->val5, 8dof_setpoint->val6, 8dof_setpoint->val7, 8dof_setpoint->val8); -} - -/** - * @brief Send a 8dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_8dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, buf, 33, 42); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, (const char *)&packet, 33, 42); -#endif -} - -#endif - -// MESSAGE 8DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 8dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_8dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field val1 from 8dof_setpoint message - * - * @return Value 1 - */ -static inline float mavlink_msg_8dof_setpoint_get_val1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field val2 from 8dof_setpoint message - * - * @return Value 2 - */ -static inline float mavlink_msg_8dof_setpoint_get_val2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field val3 from 8dof_setpoint message - * - * @return Value 3 - */ -static inline float mavlink_msg_8dof_setpoint_get_val3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field val4 from 8dof_setpoint message - * - * @return Value 4 - */ -static inline float mavlink_msg_8dof_setpoint_get_val4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field val5 from 8dof_setpoint message - * - * @return Value 5 - */ -static inline float mavlink_msg_8dof_setpoint_get_val5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field val6 from 8dof_setpoint message - * - * @return Value 6 - */ -static inline float mavlink_msg_8dof_setpoint_get_val6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field val7 from 8dof_setpoint message - * - * @return Value 7 - */ -static inline float mavlink_msg_8dof_setpoint_get_val7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field val8 from 8dof_setpoint message - * - * @return Value 8 - */ -static inline float mavlink_msg_8dof_setpoint_get_val8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a 8dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 8dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_8dof_setpoint_decode(const mavlink_message_t* msg, mavlink_8dof_setpoint_t* 8dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 8dof_setpoint->val1 = mavlink_msg_8dof_setpoint_get_val1(msg); - 8dof_setpoint->val2 = mavlink_msg_8dof_setpoint_get_val2(msg); - 8dof_setpoint->val3 = mavlink_msg_8dof_setpoint_get_val3(msg); - 8dof_setpoint->val4 = mavlink_msg_8dof_setpoint_get_val4(msg); - 8dof_setpoint->val5 = mavlink_msg_8dof_setpoint_get_val5(msg); - 8dof_setpoint->val6 = mavlink_msg_8dof_setpoint_get_val6(msg); - 8dof_setpoint->val7 = mavlink_msg_8dof_setpoint_get_val7(msg); - 8dof_setpoint->val8 = mavlink_msg_8dof_setpoint_get_val8(msg); - 8dof_setpoint->target_system = mavlink_msg_8dof_setpoint_get_target_system(msg); -#else - memcpy(8dof_setpoint, _MAV_PAYLOAD(msg), 33); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 0ba416ee1..8ddf5bf09 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp * @brief Pack a attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param roll Roll angle (rad, -pi..+pi) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a attitude struct into a message + * @brief Encode a attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + +/** * @brief Send a attitude message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 611803f2b..9f8d58759 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @brief Pack a attitude_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param q1 Quaternion component 1 @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ } /** - * @brief Encode a attitude_quaternion struct into a message + * @brief Encode a attitude_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, } /** + * @brief Encode a attitude_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); +} + +/** * @brief Send a attitude_quaternion message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 030b564c9..5703a5987 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp * @brief Pack a auth_key message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param key key * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a auth_key struct into a message + * @brief Encode a auth_key struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a auth_key struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); +} + +/** * @brief Send a auth_key message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 83c815984..c945aebae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ * @brief Pack a battery_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accu_id Accupack ID * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u } /** - * @brief Encode a battery_status struct into a message + * @brief Encode a battery_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint } /** + * @brief Encode a battery_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining); +} + +/** * @brief Send a battery_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index c7195dfca..0b6de930d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i * @brief Pack a change_operator_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System the GCS requests control for * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys } /** - * @brief Encode a change_operator_control struct into a message + * @brief Encode a change_operator_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system } /** + * @brief Encode a change_operator_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + +/** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index 5cf98e77f..c6f6a28e4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst * @brief Pack a change_operator_control_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gcs_system_id ID of the GCS this message * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t } /** - * @brief Encode a change_operator_control_ack struct into a message + * @brief Encode a change_operator_control_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy } /** + * @brief Encode a change_operator_control_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + +/** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index 82c2835de..dca2fe681 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command Command ID, as defined by MAV_CMD enum. * @param result See MAV_RESULT enum @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a command_ack struct into a message + * @brief Encode a command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); +} + +/** * @brief Send a command_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 5c44c6284..8f705c0dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t * @brief Pack a command_long message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System which should execute the command * @param target_component Component which should execute the command, 0 for all components @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a command_long struct into a message + * @brief Encode a command_long struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a command_long struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + +/** * @brief Send a command_long message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index 782ea9f26..dc0768e12 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c * @brief Pack a data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param stream_id The ID of the requested data stream * @param message_rate The requested interval between two messages of this type @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a data_stream struct into a message + * @brief Encode a data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + +/** * @brief Send a data_stream message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index a11161918..9a6ed87ee 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone * @brief Pack a debug message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param ind index of debug variable @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a debug struct into a message + * @brief Encode a debug struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo } /** + * @brief Encode a debug struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); +} + +/** * @brief Send a debug message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 5ee4e323a..6cfc75212 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @brief Pack a debug_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param name Name * @param time_usec Timestamp @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a debug_vect struct into a message + * @brief Encode a debug_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a debug_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + +/** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h index c026419a2..4f31698d5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id * @brief Pack a file_transfer_dir_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dir_path Directory path to list @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t syst } /** - * @brief Encode a file_transfer_dir_list struct into a message + * @brief Encode a file_transfer_dir_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_ } /** + * @brief Encode a file_transfer_dir_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_dir_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) +{ + return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); +} + +/** * @brief Send a file_transfer_dir_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h index 86d407d66..fc6247fac 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uin * @brief Pack a file_transfer_res message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id } /** - * @brief Encode a file_transfer_res struct into a message + * @brief Encode a file_transfer_res struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, u } /** + * @brief Encode a file_transfer_res struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_res C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) +{ + return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result); +} + +/** * @brief Send a file_transfer_res message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h index 24bf25413..05be77339 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, u * @brief Pack a file_transfer_start message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dest_path Destination path @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_ } /** - * @brief Encode a file_transfer_start struct into a message + * @brief Encode a file_transfer_start struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, } /** + * @brief Encode a file_transfer_start struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_start C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) +{ + return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); +} + +/** * @brief Send a file_transfer_start message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 11ab97ee4..7ed3d2a63 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -12,7 +12,7 @@ typedef struct __mavlink_global_position_int_t int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX } mavlink_global_position_int_t; #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 @@ -53,7 +53,7 @@ typedef struct __mavlink_global_position_int_t * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @brief Pack a global_position_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ } /** - * @brief Encode a global_position_int struct into a message + * @brief Encode a global_position_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, } /** + * @brief Encode a global_position_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + +/** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message * @@ -177,7 +191,7 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -308,7 +322,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa /** * @brief Get field hdg from global_position_int message * - * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h index 8153628aa..1a1c97199 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys * @brief Pack a global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_ } /** - * @brief Encode a global_position_setpoint_int struct into a message + * @brief Encode a global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s } /** + * @brief Encode a global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) +{ + return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); +} + +/** * @brief Send a global_position_setpoint_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index b388fa24a..f7be74c91 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t * @brief Pack a global_vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin } /** - * @brief Encode a global_vision_position_estimate struct into a message + * @brief Encode a global_vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_ } /** + * @brief Encode a global_vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); +} + +/** * @brief Send a global_vision_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index bd09cb753..016e9cb0e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin * @brief Pack a gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param latitude Latitude (WGS84), in degrees * 1E7 * @param longitude Longitude (WGS84), in degrees * 1E7 @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id } /** - * @brief Encode a gps_global_origin struct into a message + * @brief Encode a gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u } /** + * @brief Encode a gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); +} + +/** * @brief Send a gps_global_origin message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index 2136b65ee..3054d4fda 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -8,10 +8,10 @@ typedef struct __mavlink_gps_raw_int_t int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -52,10 +52,10 @@ typedef struct __mavlink_gps_raw_int_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -104,17 +104,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @brief Pack a gps_raw_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a gps_raw_int struct into a message + * @brief Encode a gps_raw_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a gps_raw_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); +} + +/** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message * @@ -182,10 +196,10 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -289,7 +303,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* /** * @brief Get field vel from gps_raw_int message * - * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* /** * @brief Get field cog from gps_raw_int message * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index cd6dde700..28d6b57d1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -86,7 +86,7 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @brief Pack a gps_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param satellites_visible Number of satellites visible * @param satellite_prn Global satellite ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a gps_status struct into a message + * @brief Encode a gps_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a gps_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + +/** * @brief Send a gps_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index b2f0b65d0..826138fad 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @brief Pack a heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM @@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a heartbeat struct into a message + * @brief Encode a heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -140,6 +140,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 6e7492ea6..0dcd95ed3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @brief Pack a highres_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a highres_imu struct into a message + * @brief Encode a highres_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -214,6 +214,20 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a highres_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); +} + +/** * @brief Send a highres_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index 3319d3fd2..aed5108d0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t * @brief Pack a hil_controls message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll_ailerons Control output -1 .. 1 @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a hil_controls struct into a message + * @brief Encode a hil_controls struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a hil_controls struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + +/** * @brief Send a hil_controls message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 75ab6835d..36a551872 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @brief Pack a hil_gps message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hil_gps struct into a message + * @brief Encode a hil_gps struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -198,6 +198,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a hil_gps struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + +/** * @brief Send a hil_gps message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index 13e13d47c..acb1392e1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint * @brief Pack a hil_optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, } /** - * @brief Encode a hil_optical_flow struct into a message + * @brief Encode a hil_optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, ui } /** + * @brief Encode a hil_optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); +} + +/** * @brief Send a hil_optical_flow message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index f2435dde8..a42bde50b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin * @brief Pack a hil_rc_inputs_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param chan1_raw RC channel 1 value, in microseconds @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id } /** - * @brief Encode a hil_rc_inputs_raw struct into a message + * @brief Encode a hil_rc_inputs_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -206,6 +206,20 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u } /** + * @brief Encode a hil_rc_inputs_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + +/** * @brief Send a hil_rc_inputs_raw message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 422e55adc..6c2667473 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @brief Pack a hil_sensor message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a hil_sensor struct into a message + * @brief Encode a hil_sensor struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -214,6 +214,20 @@ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a hil_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + +/** * @brief Send a hil_sensor message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index 1d3f28664..bcc857767 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -134,7 +134,7 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com * @brief Pack a hil_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) @@ -209,7 +209,7 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a hil_state struct into a message + * @brief Encode a hil_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -222,6 +222,20 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a hil_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** * @brief Send a hil_state message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 0474e64a2..732176193 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -132,7 +132,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, * @brief Pack a hil_state_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion @@ -205,7 +205,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system } /** - * @brief Encode a hil_state_quaternion struct into a message + * @brief Encode a hil_state_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -218,6 +218,20 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id } /** + * @brief Encode a hil_state_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + +/** * @brief Send a hil_state_quaternion message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index 56723f3d7..a0b72c0e1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui * @brief Pack a local_position_ned message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i } /** - * @brief Encode a local_position_ned struct into a message + * @brief Encode a local_position_ned struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, } /** + * @brief Encode a local_position_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + +/** * @brief Send a local_position_ned message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index c206a2906..8c4686202 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack( * @brief Pack a local_position_ned_system_global_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_ } /** - * @brief Encode a local_position_ned_system_global_offset struct into a message + * @brief Encode a local_position_ned_system_global_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod } /** + * @brief Encode a local_position_ned_system_global_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + +/** * @brief Send a local_position_ned_system_global_offset message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h index 96f35fe62..1794815f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i * @brief Pack a local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys } /** - * @brief Encode a local_position_setpoint struct into a message + * @brief Encode a local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system } /** + * @brief Encode a local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) +{ + return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); +} + +/** * @brief Send a local_position_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index c9e4a4f8a..6b6e9e148 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @brief Pack a manual_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled. * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u } /** - * @brief Encode a manual_control struct into a message + * @brief Encode a manual_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint } /** + * @brief Encode a manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); +} + +/** * @brief Send a manual_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index d59e21292..a694947c1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8 * @brief Pack a manual_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll rate in radians per second @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, } /** - * @brief Encode a manual_setpoint struct into a message + * @brief Encode a manual_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uin } /** + * @brief Encode a manual_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + +/** * @brief Send a manual_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index f8ae21b05..5f79329c2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c * @brief Pack a memory_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param address Starting address of the debug variables * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a memory_vect struct into a message + * @brief Encode a memory_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a memory_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 32825647f..7421d8394 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a mission_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a mission_ack struct into a message + * @brief Encode a mission_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a mission_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); +} + +/** * @brief Send a mission_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 06d2ac2e7..8f441c8e5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin * @brief Pack a mission_clear_all message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id } /** - * @brief Encode a mission_clear_all struct into a message + * @brief Encode a mission_clear_all struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u } /** + * @brief Encode a mission_clear_all struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); +} + +/** * @brief Send a mission_clear_all message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index b28cec6f6..eac779306 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @brief Pack a mission_count message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mission_count struct into a message + * @brief Encode a mission_count struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 } /** + * @brief Encode a mission_count struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); +} + +/** * @brief Send a mission_count message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index 5bf0899be..dbcdbd3f9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8 * @brief Pack a mission_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_current struct into a message + * @brief Encode a mission_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin } /** + * @brief Encode a mission_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); +} + +/** * @brief Send a mission_current message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index ed9d6e4af..634f80c37 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @brief Pack a mission_item message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mission_item struct into a message + * @brief Encode a mission_item struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -206,6 +206,20 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a mission_item struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); +} + +/** * @brief Send a mission_item message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index 3f8a51a13..f3744fde6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, * @brief Pack a mission_item_reached message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system } /** - * @brief Encode a mission_item_reached struct into a message + * @brief Encode a mission_item_reached struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id } /** + * @brief Encode a mission_item_reached struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); +} + +/** * @brief Send a mission_item_reached message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index 0ced17614..ac84e3554 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 * @brief Pack a mission_request message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_request struct into a message + * @brief Encode a mission_request struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin } /** + * @brief Encode a mission_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); +} + +/** * @brief Send a mission_request message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index 391df7f4d..d999babdb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, * @brief Pack a mission_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system } /** - * @brief Encode a mission_request_list struct into a message + * @brief Encode a mission_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id } /** + * @brief Encode a mission_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); +} + +/** * @brief Send a mission_request_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index d5a1c6939..35c7e1285 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys * @brief Pack a mission_request_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ } /** - * @brief Encode a mission_request_partial_list struct into a message + * @brief Encode a mission_request_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s } /** + * @brief Encode a mission_request_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + +/** * @brief Send a mission_request_partial_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 2e145aa3e..63b37cf8c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, u * @brief Pack a mission_set_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_ } /** - * @brief Encode a mission_set_current struct into a message + * @brief Encode a mission_set_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, } /** + * @brief Encode a mission_set_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + +/** * @brief Send a mission_set_current message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 6342f6038..7de38bd7b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste * @brief Pack a mission_write_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t } /** - * @brief Encode a mission_write_partial_list struct into a message + * @brief Encode a mission_write_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys } /** + * @brief Encode a mission_write_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + +/** * @brief Send a mission_write_partial_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index a9d28a3d0..fbf4f75c9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin * @brief Pack a named_value_float message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id } /** - * @brief Encode a named_value_float struct into a message + * @brief Encode a named_value_float struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u } /** + * @brief Encode a named_value_float struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + +/** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index ea53ea888..052f24793 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 * @brief Pack a named_value_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, } /** - * @brief Encode a named_value_int struct into a message + * @brief Encode a named_value_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin } /** + * @brief Encode a named_value_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + +/** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e9fa0f522..e95c144de 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, * @brief Pack a nav_controller_output message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param nav_roll Current desired roll in degrees * @param nav_pitch Current desired pitch in degrees @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste } /** - * @brief Encode a nav_controller_output struct into a message + * @brief Encode a nav_controller_output struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i } /** + * @brief Encode a nav_controller_output struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + +/** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h index c0e765a44..4debb6e66 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, * @brief Pack a omnidirectional_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID @@ -126,7 +126,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system } /** - * @brief Encode a omnidirectional_flow struct into a message + * @brief Encode a omnidirectional_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -139,6 +139,20 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id } /** + * @brief Encode a omnidirectional_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param omnidirectional_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) +{ + return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); +} + +/** * @brief Send a omnidirectional_flow message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index e01dc5e79..cf6db9147 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -8,8 +8,8 @@ typedef struct __mavlink_optical_flow_t float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction + int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) + int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) uint8_t sensor_id; ///< Sensor ID uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality } mavlink_optical_flow_t; @@ -45,8 +45,8 @@ typedef struct __mavlink_optical_flow_t * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -94,12 +94,12 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @brief Pack a optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a optical_flow struct into a message + * @brief Encode a optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,13 +158,27 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** * @brief Send a optical_flow message * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -237,7 +251,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa /** * @brief Get field flow_x from optical_flow message * - * @return Flow in pixels in x-sensor direction + * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) { @@ -247,7 +261,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_ /** * @brief Get field flow_y from optical_flow message * - * @return Flow in pixels in y-sensor direction + * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index da61181b2..39e007274 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @brief Pack a param_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_list struct into a message + * @brief Encode a param_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, } /** + * @brief Encode a param_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 6b1568026..5d9113114 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @brief Pack a param_request_read message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_read struct into a message + * @brief Encode a param_request_read struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, } /** + * @brief Encode a param_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 66b0f6629..1bd1f0059 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @brief Pack a param_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a param_set struct into a message + * @brief Encode a param_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a param_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + +/** * @brief Send a param_set message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 599139374..17c759811 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @brief Pack a param_value message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string * @param param_value Onboard parameter value @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a param_value struct into a message + * @brief Encode a param_value struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a param_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + +/** * @brief Send a param_value message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 5a4c50907..0c68ca176 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @brief Pack a ping message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Unix timestamp in microseconds * @param seq PING sequence @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ping struct into a message + * @brief Encode a ping struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a ping struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + +/** * @brief Send a ping message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index 06e6a5542..fceb7d168 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t * @brief Pack a radio_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a radio_status struct into a message + * @brief Encode a radio_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a radio_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + +/** * @brief Send a radio_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index ce8863647..62a9b6eea 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param xacc X acceleration (raw) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_imu struct into a message + * @brief Encode a raw_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a raw_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + +/** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index dcc2cbe06..82c5fad4a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t * @brief Pack a raw_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param press_abs Absolute pressure (raw) @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a raw_pressure struct into a message + * @brief Encode a raw_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a raw_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + +/** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 9854a190c..0d8a1514b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -4,14 +4,14 @@ typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID } mavlink_rc_channels_override_t; @@ -49,14 +49,14 @@ typedef struct __mavlink_rc_channels_override_t * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -104,18 +104,18 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @brief Pack a rc_channels_override message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system } /** - * @brief Encode a rc_channels_override struct into a message + * @brief Encode a rc_channels_override struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,19 +174,33 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id } /** + * @brief Encode a rc_channels_override struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -259,7 +273,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons /** * @brief Get field chan1_raw from rc_channels_override message * - * @return RC channel 1 value, in microseconds + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { @@ -269,7 +283,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl /** * @brief Get field chan2_raw from rc_channels_override message * - * @return RC channel 2 value, in microseconds + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { @@ -279,7 +293,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl /** * @brief Get field chan3_raw from rc_channels_override message * - * @return RC channel 3 value, in microseconds + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { @@ -289,7 +303,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl /** * @brief Get field chan4_raw from rc_channels_override message * - * @return RC channel 4 value, in microseconds + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl /** * @brief Get field chan5_raw from rc_channels_override message * - * @return RC channel 5 value, in microseconds + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl /** * @brief Get field chan6_raw from rc_channels_override message * - * @return RC channel 6 value, in microseconds + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl /** * @brief Get field chan7_raw from rc_channels_override message * - * @return RC channel 7 value, in microseconds + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { @@ -329,7 +343,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl /** * @brief Get field chan8_raw from rc_channels_override message * - * @return RC channel 8 value, in microseconds + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 4c1315bed..3c0aed020 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_raw_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_raw_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_raw_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @brief Pack a rc_channels_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a rc_channels_raw struct into a message + * @brief Encode a rc_channels_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin } /** + * @brief Encode a rc_channels_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + +/** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message /** * @brief Get field chan1_raw from rc_channels_raw message * - * @return RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m /** * @brief Get field chan2_raw from rc_channels_raw message * - * @return RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m /** * @brief Get field chan3_raw from rc_channels_raw message * - * @return RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m /** * @brief Get field chan4_raw from rc_channels_raw message * - * @return RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m /** * @brief Get field chan5_raw from rc_channels_raw message * - * @return RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m /** * @brief Get field chan6_raw from rc_channels_raw message * - * @return RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m /** * @brief Get field chan7_raw from rc_channels_raw message * - * @return RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m /** * @brief Get field chan8_raw from rc_channels_raw message * - * @return RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index be6322bcd..2153ab392 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_scaled_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_scaled_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_scaled_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @brief Pack a rc_channels_scaled message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i } /** - * @brief Encode a rc_channels_scaled struct into a message + * @brief Encode a rc_channels_scaled struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, } /** + * @brief Encode a rc_channels_scaled struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + +/** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_mess /** * @brief Get field chan1_scaled from rc_channels_scaled message * - * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl /** * @brief Get field chan2_scaled from rc_channels_scaled message * - * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl /** * @brief Get field chan3_scaled from rc_channels_scaled message * - * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl /** * @brief Get field chan4_scaled from rc_channels_scaled message * - * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl /** * @brief Get field chan5_scaled from rc_channels_scaled message * - * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl /** * @brief Get field chan6_scaled from rc_channels_scaled message * - * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl /** * @brief Get field chan7_scaled from rc_channels_scaled message * - * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl /** * @brief Get field chan8_scaled from rc_channels_scaled message * - * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index ee21d1fe0..c754ad876 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @brief Pack a request_data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ } /** - * @brief Encode a request_data_stream struct into a message + * @brief Encode a request_data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, } /** + * @brief Encode a request_data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + +/** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h index a7e9df94b..ac3ef4fa9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_rate Desired roll rate in radians per second @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(u } /** + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 517797655..626477322 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_speed Desired roll angular speed in rad/s @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u } /** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index aee036022..ffcdc547b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll angle in radians @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint } /** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t } /** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index 100fabf16..fcd54cbb7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u * @brief Pack a safety_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. * @param p1x x position 1 / Latitude 1 @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ } /** - * @brief Encode a safety_allowed_area struct into a message + * @brief Encode a safety_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, } /** + * @brief Encode a safety_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index d365b7aed..61f6af155 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i * @brief Pack a safety_set_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys } /** - * @brief Encode a safety_set_allowed_area struct into a message + * @brief Encode a safety_set_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system } /** + * @brief Encode a safety_set_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 2751ddfe7..3010d051a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @brief Pack a scaled_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param xacc X acceleration (mg) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a scaled_imu struct into a message + * @brief Encode a scaled_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a scaled_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + +/** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index f54e28195..10324bc94 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @brief Pack a scaled_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param press_abs Absolute pressure (hectopascal) @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, } /** - * @brief Encode a scaled_pressure struct into a message + * @brief Encode a scaled_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin } /** + * @brief Encode a scaled_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + +/** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 10bdcbc8c..6a14e93ed 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @brief Pack a servo_output_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a servo_output_raw struct into a message + * @brief Encode a servo_output_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui } /** + * @brief Encode a servo_output_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h index 0364b4241..6f0d7a69d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t * @brief Pack a set_global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui } /** - * @brief Encode a set_global_position_setpoint_int struct into a message + * @brief Encode a set_global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8 } /** + * @brief Encode a set_global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) +{ + return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); +} + +/** * @brief Send a set_global_position_setpoint_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index e3cd4f441..c444d8d52 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, * @brief Pack a set_gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste } /** - * @brief Encode a set_gps_global_origin struct into a message + * @brief Encode a set_gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i } /** + * @brief Encode a set_gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); +} + +/** * @brief Send a set_gps_global_origin message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h index b92c0560e..6f2835e03 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst * @brief Pack a set_local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t } /** - * @brief Encode a set_local_position_setpoint struct into a message + * @brief Encode a set_local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy } /** + * @brief Encode a set_local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) +{ + return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); +} + +/** * @brief Send a set_local_position_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 08ec73309..1aff42cce 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp * @brief Pack a set_mode message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The system setting the mode * @param base_mode The new base mode @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a set_mode struct into a message + * @brief Encode a set_mode struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a set_mode struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + +/** * @brief Send a set_mode message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h index b79114e1a..8ceb8888f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_ * @brief Pack a set_quad_motors_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID of the system that should set these motor commands * @param motor_front_nw Front motor in + configuration, front left motor in x configuration @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t sy } /** - * @brief Encode a set_quad_motors_setpoint struct into a message + * @brief Encode a set_quad_motors_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t syste } /** + * @brief Encode a set_quad_motors_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_motors_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) +{ + return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); +} + +/** * @brief Send a set_quad_motors_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h index 06223845f..9ef294cc9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) uint8_t led_red[4]; ///< RGB red channel (0-255) @@ -56,10 +56,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -103,17 +103,17 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -155,7 +155,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack } /** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -168,6 +168,20 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco } /** + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * @@ -176,10 +190,10 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -278,7 +292,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -288,7 +302,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -298,7 +312,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -308,7 +322,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h index 6c62b3530..7d8d526f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) } mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t; @@ -44,10 +44,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -85,14 +85,14 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uin * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -128,7 +128,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_cha } /** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,15 +141,29 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(u } /** + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -212,7 +226,7 @@ static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode( /** * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -222,7 +236,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll /** * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -232,7 +246,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitc /** * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -242,7 +256,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw( /** * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index c379a75d9..5846ba41f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin } /** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ } /** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + +/** * @brief Send a set_roll_pitch_yaw_speed_thrust message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 146891eaf..334fd39e3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system * @brief Pack a set_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s } /** - * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst } /** + * @brief Encode a set_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h index f352617a2..93ce345b6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_6dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param trans_x Translational Component in x @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_6dof struct into a message + * @brief Encode a setpoint_6dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8 } /** + * @brief Encode a setpoint_6dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_6dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) +{ + return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); +} + +/** * @brief Send a setpoint_6dof message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h index d7622b696..de80e4645 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_8dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param val1 Value 1 @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_8dof struct into a message + * @brief Encode a setpoint_8dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8 } /** + * @brief Encode a setpoint_8dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_8dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) +{ + return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); +} + +/** * @brief Send a setpoint_8dof message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index 6fd32abd2..ebd657cf3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com * @brief Pack a sim_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param q1 True attitude quaternion component 1 * @param q2 True attitude quaternion component 2 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a sim_state struct into a message + * @brief Encode a sim_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -262,6 +262,20 @@ static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a sim_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); +} + +/** * @brief Send a sim_state message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h index 8a002fc11..2db627b96 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint * @brief Pack a state_correction message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param xErr x position error * @param yErr y position error @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, } /** - * @brief Encode a state_correction struct into a message + * @brief Encode a state_correction struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui } /** + * @brief Encode a state_correction struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param state_correction C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) +{ + return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); +} + +/** * @brief Send a state_correction message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 103486863..536ca0634 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @brief Pack a statustext message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. * @param text Status text message, without null termination character @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a statustext struct into a message + * @brief Encode a statustext struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -106,6 +106,20 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a statustext struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); +} + +/** * @brief Send a statustext message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 916bc4f07..058c630dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @brief Pack a sys_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a sys_status struct into a message + * @brief Encode a sys_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -198,6 +198,20 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a sys_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); +} + +/** * @brief Send a sys_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index b235fe205..1807567ae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c * @brief Pack a system_time message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a system_time struct into a message + * @brief Encode a system_time struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a system_time struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + +/** * @brief Send a system_time message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 9d459921f..5b1093a3d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo * @brief Pack a vfr_hud message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airspeed Current airspeed in m/s * @param groundspeed Current ground speed in m/s @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a vfr_hud struct into a message + * @brief Encode a vfr_hud struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a vfr_hud struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + +/** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index 75e4b5b7a..a254202e4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @brief Pack a vicon_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys } /** - * @brief Encode a vicon_position_estimate struct into a message + * @brief Encode a vicon_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system } /** + * @brief Encode a vicon_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); +} + +/** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index 47ccb11ec..f7a741b09 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @brief Pack a vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy } /** - * @brief Encode a vision_position_estimate struct into a message + * @brief Encode a vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste } /** + * @brief Encode a vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); +} + +/** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index c38eee62c..660225128 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @brief Pack a vision_speed_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X speed @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste } /** - * @brief Encode a vision_speed_estimate struct into a message + * @brief Encode a vision_speed_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i } /** + * @brief Encode a vision_speed_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); +} + +/** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h index c91a641de..3c354f422 100644 --- a/mavlink/include/mavlink/v1.0/common/version.h +++ b/mavlink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:47:40 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:05 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h index f5576255e..f590cda80 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h @@ -77,6 +77,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -85,7 +86,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h index bd0be342f..bc47a547a 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com * @brief Pack a airspeeds message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param airspeed_imu Airspeed estimate from IMU, cm/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a airspeeds struct into a message + * @brief Encode a airspeeds struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a airspeeds struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeeds C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) +{ + return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); +} + +/** * @brief Send a airspeeds message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h index c1ce66875..e64787cc7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com * @brief Pack a altitudes message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a altitudes struct into a message + * @brief Encode a altitudes struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a altitudes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param altitudes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) +{ + return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); +} + +/** * @brief Send a altitudes message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h index d72a4dcac..581bd35dc 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy * @brief Pack a flexifunction_buffer_function message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -133,7 +133,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8 } /** - * @brief Encode a flexifunction_buffer_function struct into a message + * @brief Encode a flexifunction_buffer_function struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -146,6 +146,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t } /** + * @brief Encode a flexifunction_buffer_function struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) +{ + return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); +} + +/** * @brief Send a flexifunction_buffer_function message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h index 58f1786ef..790afd52b 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_ * @brief Pack a flexifunction_buffer_function_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(u } /** - * @brief Encode a flexifunction_buffer_function_ack struct into a message + * @brief Encode a flexifunction_buffer_function_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint } /** + * @brief Encode a flexifunction_buffer_function_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) +{ + return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); +} + +/** * @brief Send a flexifunction_buffer_function_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h index 2f6668cf9..ce722c8a4 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, * @brief Pack a flexifunction_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t syste } /** - * @brief Encode a flexifunction_command struct into a message + * @brief Encode a flexifunction_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_i } /** + * @brief Encode a flexifunction_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) +{ + return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); +} + +/** * @brief Send a flexifunction_command message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h index 4798febb0..070dc4bf8 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system * @brief Pack a flexifunction_command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command_type Command acknowledged * @param result result of acknowledge @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t s } /** - * @brief Encode a flexifunction_command_ack struct into a message + * @brief Encode a flexifunction_command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t syst } /** + * @brief Encode a flexifunction_command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) +{ + return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); +} + +/** * @brief Send a flexifunction_command_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h index 947bfc591..ef262a6b1 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h @@ -82,7 +82,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_i * @brief Pack a flexifunction_directory message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -125,7 +125,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t sys } /** - * @brief Encode a flexifunction_directory struct into a message + * @brief Encode a flexifunction_directory struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -138,6 +138,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system } /** + * @brief Encode a flexifunction_directory struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) +{ + return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); +} + +/** * @brief Send a flexifunction_directory message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h index 5489dd6b5..d3a386ce5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst * @brief Pack a flexifunction_directory_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t } /** - * @brief Encode a flexifunction_directory_ack struct into a message + * @brief Encode a flexifunction_directory_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t sy } /** + * @brief Encode a flexifunction_directory_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) +{ + return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); +} + +/** * @brief Send a flexifunction_directory_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h index 9ffc2caa5..e50f77b08 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id * @brief Pack a flexifunction_read_req message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t syst } /** - * @brief Encode a flexifunction_read_req struct into a message + * @brief Encode a flexifunction_read_req struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_ } /** + * @brief Encode a flexifunction_read_req struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_read_req C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) +{ + return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); +} + +/** * @brief Send a flexifunction_read_req message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h index fc7d1d2f8..41afb3811 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uin * @brief Pack a flexifunction_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id } /** - * @brief Encode a flexifunction_set struct into a message + * @brief Encode a flexifunction_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, u } /** + * @brief Encode a flexifunction_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) +{ + return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component); +} + +/** * @brief Send a flexifunction_set message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h index df5645e76..d21ab4816 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f13 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_week_no Serial UDB Extra GPS Week Number * @param sue_lat_origin Serial UDB Extra MP Origin Latitude @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f13 struct into a message + * @brief Encode a serial_udb_extra_f13 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f13 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f13 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) +{ + return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); +} + +/** * @brief Send a serial_udb_extra_f13 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h index 5e38a590a..6ac12f28a 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f14 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f14 struct into a message + * @brief Encode a serial_udb_extra_f14 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f14 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f14 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) +{ + return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); +} + +/** * @brief Send a serial_udb_extra_f14 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h index 8779b25ff..10c3f4ca4 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f15 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f15 struct into a message + * @brief Encode a serial_udb_extra_f15 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -111,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f15 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f15 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) +{ + return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); +} + +/** * @brief Send a serial_udb_extra_f15 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h index 1a173bfe4..659e6b7c5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f16 struct into a message + * @brief Encode a serial_udb_extra_f16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -111,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) +{ + return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); +} + +/** * @brief Send a serial_udb_extra_f16 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h index ddfc236ba..15ba68a34 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_a message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_status Serial UDB Extra Status @@ -305,7 +305,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_a struct into a message + * @brief Encode a serial_udb_extra_f2_a struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -318,6 +318,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_i } /** + * @brief Encode a serial_udb_extra_f2_a struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_a C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) +{ + return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); +} + +/** * @brief Send a serial_udb_extra_f2_a message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h index 74da8416d..7cb8c87da 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h @@ -219,7 +219,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_b message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 @@ -345,7 +345,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_b struct into a message + * @brief Encode a serial_udb_extra_f2_b struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -358,6 +358,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_i } /** + * @brief Encode a serial_udb_extra_f2_b struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_b C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) +{ + return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); +} + +/** * @brief Send a serial_udb_extra_f2_b message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h index 83ccbbedb..77c616cc2 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f4 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f4 struct into a message + * @brief Encode a serial_udb_extra_f4 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f4 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f4 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) +{ + return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); +} + +/** * @brief Send a serial_udb_extra_f4 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h index 2b2451f00..e115a9b44 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f5 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f5 struct into a message + * @brief Encode a serial_udb_extra_f5 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f5 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f5 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) +{ + return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); +} + +/** * @brief Send a serial_udb_extra_f5 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h index 9d58ca9a8..656103d09 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f6 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f6 struct into a message + * @brief Encode a serial_udb_extra_f6 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f6 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f6 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) +{ + return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); +} + +/** * @brief Send a serial_udb_extra_f6 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h index ab73b967e..51c98e6b7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f7 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f7 struct into a message + * @brief Encode a serial_udb_extra_f7 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f7 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f7 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) +{ + return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); +} + +/** * @brief Send a serial_udb_extra_f7 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h index 310246e8e..8557a95e2 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f8 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f8 struct into a message + * @brief Encode a serial_udb_extra_f8 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f8 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f8 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) +{ + return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); +} + +/** * @brief Send a serial_udb_extra_f8 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index ea0265298..59f3d2ded 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:50 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:51 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index d89363577..51afac87c 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -9,6 +9,10 @@ #endif #include <math.h> +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + /** * @file mavlink_conversions.h * @@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo float phi, theta, psi; theta = asin(-dcm[2][0]); - if (fabs(theta - M_PI_2) < 1.0e-3f) { + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], + psi = (atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); @@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif
\ No newline at end of file +#endif diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 72cf91fb9..96672f847 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -16,7 +16,12 @@ #ifndef MAVLINK_GET_CHANNEL_STATUS MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { +#if MAVLINK_EXTERNAL_RX_STATUS + // No m_mavlink_status array defined in function, + // has to be defined externally +#else static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#endif return &m_mavlink_status[chan]; } #endif @@ -29,11 +34,8 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { #if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_message array defined in function, + // No m_mavlink_buffer array defined in function, // has to be defined externally -#ifndef m_mavlink_message -#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];) -#endif #else static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; #endif diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h index 1aac8395d..ef5354d5e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint * @brief Pack a attitude_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled * @param roll roll @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a attitude_control struct into a message + * @brief Encode a attitude_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui } /** + * @brief Encode a attitude_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) +{ + return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); +} + +/** * @brief Send a attitude_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h index 353e10581..d41093792 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @brief Pack a brief_feature message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param x x position in m * @param y y position in m @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a brief_feature struct into a message + * @brief Encode a brief_feature struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -154,6 +154,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 } /** + * @brief Encode a brief_feature struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param brief_feature C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) +{ + return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); +} + +/** * @brief Send a brief_feature message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h index 5251c35c3..a7af8c8eb 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst * @brief Pack a data_transmission_handshake message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) * @param size total data size in bytes (set on ACK only) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t } /** - * @brief Encode a data_transmission_handshake struct into a message + * @brief Encode a data_transmission_handshake struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy } /** + * @brief Encode a data_transmission_handshake struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + +/** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h index feeef6f39..40001dc30 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin * @brief Pack a encapsulated_data message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id } /** - * @brief Encode a encapsulated_data struct into a message + * @brief Encode a encapsulated_data struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -106,6 +106,20 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u } /** + * @brief Encode a encapsulated_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + +/** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h index 6d1d9cca7..ae4db825d 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 * @brief Pack a image_available message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_id Camera id * @param cam_no Camera # (starts with 0) @@ -265,7 +265,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_available struct into a message + * @brief Encode a image_available struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -278,6 +278,20 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin } /** + * @brief Encode a image_available struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_available C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available) +{ + return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); +} + +/** * @brief Send a image_available message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h index 784cedf8b..664574be9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, * @brief Pack a image_trigger_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param enable 0 to disable, 1 to enable * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste } /** - * @brief Encode a image_trigger_control struct into a message + * @brief Encode a image_trigger_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i } /** + * @brief Encode a image_trigger_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_trigger_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) +{ + return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable); +} + +/** * @brief Send a image_trigger_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h index 05b0d775f..f3a2243af 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 * @brief Pack a image_triggered message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param timestamp Timestamp * @param seq IMU seq @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_triggered struct into a message + * @brief Encode a image_triggered struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -190,6 +190,20 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin } /** + * @brief Encode a image_triggered struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_triggered C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) +{ + return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); +} + +/** * @brief Send a image_triggered message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h index 817ec60a2..6bdcceaf9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon * @brief Pack a marker message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID * @param x x position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a marker struct into a message + * @brief Encode a marker struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a marker struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param marker C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker) +{ + return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); +} + +/** * @brief Send a marker message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h index 7e29d7152..aa7b827a3 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @brief Pack a pattern_detected message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Pattern, 1: Letter * @param confidence Confidence of detection @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, } /** - * @brief Encode a pattern_detected struct into a message + * @brief Encode a pattern_detected struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui } /** + * @brief Encode a pattern_detected struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pattern_detected C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) +{ + return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); +} + +/** * @brief Send a pattern_detected message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h index a6faebbb5..913a52897 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @brief Pack a point_of_interest message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id } /** - * @brief Encode a point_of_interest struct into a message + * @brief Encode a point_of_interest struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -154,6 +154,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u } /** + * @brief Encode a point_of_interest struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) +{ + return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); +} + +/** * @brief Send a point_of_interest message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h index 8d02f9e5c..e24436431 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @brief Pack a point_of_interest_connection message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -165,7 +165,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ } /** - * @brief Encode a point_of_interest_connection struct into a message + * @brief Encode a point_of_interest_connection struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -178,6 +178,20 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s } /** + * @brief Encode a point_of_interest_connection struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest_connection C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) +{ + return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); +} + +/** * @brief Send a point_of_interest_connection message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h index 6c86be3ab..6f4ca510a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system * @brief Pack a position_control_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID of waypoint, 0 for plain position * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s } /** - * @brief Encode a position_control_setpoint struct into a message + * @brief Encode a position_control_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst } /** + * @brief Encode a position_control_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_control_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) +{ + return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); +} + +/** * @brief Send a position_control_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h index 0a0dbdb37..08cedbb4b 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_aux message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_aux struct into a message + * @brief Encode a raw_aux struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a raw_aux struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_aux C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) +{ + return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); +} + +/** * @brief Send a raw_aux message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h index 7be640911..b26d748c2 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 * @brief Pack a set_cam_shutter message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_no Camera id * @param cam_mode Camera mode: 0 = auto, 1 = manual @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_cam_shutter struct into a message + * @brief Encode a set_cam_shutter struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin } /** + * @brief Encode a set_cam_shutter struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_cam_shutter C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) +{ + return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); +} + +/** * @brief Send a set_cam_shutter message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h index 25bff659e..f1f9e698f 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst * @brief Pack a set_position_control_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t } /** - * @brief Encode a set_position_control_offset struct into a message + * @brief Encode a set_position_control_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t sy } /** + * @brief Encode a set_position_control_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_control_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) +{ + return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); +} + +/** * @brief Send a set_position_control_offset message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h index 426788018..9458087da 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint * @brief Pack a watchdog_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system_id Target system ID * @param watchdog_id Watchdog ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, } /** - * @brief Encode a watchdog_command struct into a message + * @brief Encode a watchdog_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui } /** + * @brief Encode a watchdog_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) +{ + return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); +} + +/** * @brief Send a watchdog_command message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h index 49478999c..3f4295ee5 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui * @brief Pack a watchdog_heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_count Number of processes @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i } /** - * @brief Encode a watchdog_heartbeat struct into a message + * @brief Encode a watchdog_heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, } /** + * @brief Encode a watchdog_heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) +{ + return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); +} + +/** * @brief Send a watchdog_heartbeat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h index d706ef85e..55853cdde 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h @@ -78,7 +78,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @brief Pack a watchdog_process_info message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste } /** - * @brief Encode a watchdog_process_info struct into a message + * @brief Encode a watchdog_process_info struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -131,6 +131,20 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i } /** + * @brief Encode a watchdog_process_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) +{ + return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); +} + +/** * @brief Send a watchdog_process_info message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h index b1bbaaae7..a0410d803 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i * @brief Pack a watchdog_process_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys } /** - * @brief Encode a watchdog_process_status struct into a message + * @brief Encode a watchdog_process_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system } /** + * @brief Encode a watchdog_process_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) +{ + return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); +} + +/** * @brief Send a watchdog_process_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index a2d0d0e14..3fc9e6477 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -72,6 +72,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -80,7 +81,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index 61fe6e930..07831a425 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:05 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:13 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h index ddacae59c..86e7ff588 100644 --- a/mavlink/include/mavlink/v1.0/protocol.h +++ b/mavlink/include/mavlink/v1.0/protocol.h @@ -162,7 +162,7 @@ static inline void byte_copy_8(char *dst, const char *src) /* like memcpy(), but if src is NULL, do a memset to zero */ -static void mav_array_memcpy(void *dest, const void *src, size_t n) +static inline void mav_array_memcpy(void *dest, const void *src, size_t n) { if (src == NULL) { memset(dest, 0, n); diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h index 5cdd58456..73e9d75db 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint * @brief Pack a cmd_airspeed_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param spCmd @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, } /** - * @brief Encode a cmd_airspeed_ack struct into a message + * @brief Encode a cmd_airspeed_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, ui } /** + * @brief Encode a cmd_airspeed_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) +{ + return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); +} + +/** * @brief Send a cmd_airspeed_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h index 2a4894fe7..b6b567f99 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uin * @brief Pack a cmd_airspeed_chng message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id } /** - * @brief Encode a cmd_airspeed_chng struct into a message + * @brief Encode a cmd_airspeed_chng struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, u } /** + * @brief Encode a cmd_airspeed_chng struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_chng C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) +{ + return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); +} + +/** * @brief Send a cmd_airspeed_chng message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h index d600e9174..642f7aab9 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t * @brief Pack a filt_rot_vel message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rotVel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a filt_rot_vel struct into a message + * @brief Encode a filt_rot_vel struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a filt_rot_vel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param filt_rot_vel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) +{ + return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel); +} + +/** * @brief Send a filt_rot_vel message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h index cb1c86f43..0f8369881 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t compo * @brief Pack a llc_out message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param servoOut @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a llc_out struct into a message + * @brief Encode a llc_out struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -123,6 +123,20 @@ static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a llc_out struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param llc_out C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) +{ + return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut); +} + +/** * @brief Send a llc_out message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h index b6bd0d9e5..5d9382326 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t * @brief Pack a obs_air_temp message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airT @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_air_temp struct into a message + * @brief Encode a obs_air_temp struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_air_temp struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_temp C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) +{ + return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); +} + +/** * @brief Send a obs_air_temp message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h index 87a65cfa0..35d813ca1 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint * @brief Pack a obs_air_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param magnitude @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, } /** - * @brief Encode a obs_air_velocity struct into a message + * @brief Encode a obs_air_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -136,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, ui } /** + * @brief Encode a obs_air_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) +{ + return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); +} + +/** * @brief Send a obs_air_velocity message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h index 602eafc80..9c80cd66e 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t * @brief Pack a obs_attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param quat @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_attitude struct into a message + * @brief Encode a obs_attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) +{ + return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat); +} + +/** * @brief Send a obs_attitude message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h index 3ab855ba8..24dd43b57 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_bias message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accBias @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_bias struct into a message + * @brief Encode a obs_bias struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -123,6 +123,20 @@ static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a obs_bias struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_bias C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) +{ + return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias); +} + +/** * @brief Send a obs_bias message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h index ec494d51a..cfc2fe7e1 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t * @brief Pack a obs_position message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param lon @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_position struct into a message + * @brief Encode a obs_position struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -136,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_position struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) +{ + return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt); +} + +/** * @brief Send a obs_position message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h index e42b0261f..24e272bf7 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t compo * @brief Pack a obs_qff message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param qff @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_qff struct into a message + * @brief Encode a obs_qff struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a obs_qff struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_qff C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) +{ + return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); +} + +/** * @brief Send a obs_qff message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h index 2fa6e9111..6e3776632 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t * @brief Pack a obs_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param vel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_velocity struct into a message + * @brief Encode a obs_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) +{ + return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); +} + +/** * @brief Send a obs_velocity message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h index bc9f6e4ef..55f7cb2ae 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param wind @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_wind struct into a message + * @brief Encode a obs_wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a obs_wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) +{ + return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); +} + +/** * @brief Send a obs_wind message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h index 0799af07f..e0963ece7 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t compo * @brief Pack a pm_elec message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param PwCons @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a pm_elec struct into a message + * @brief Encode a pm_elec struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -132,6 +132,20 @@ static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a pm_elec struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pm_elec C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) +{ + return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); +} + +/** * @brief Send a pm_elec message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h index e5e483a73..94f3e58bb 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h @@ -90,7 +90,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t comp * @brief Pack a sys_stat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gps @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a sys_stat struct into a message + * @brief Encode a sys_stat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a sys_stat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_stat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) +{ + return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); +} + +/** * @brief Send a sys_stat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index 4633e6eb5..ab3f47b9d 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:22 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:02 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/misc/tones/charge.txt b/misc/tones/charge.txt new file mode 100644 index 000000000..cf33c58b3 --- /dev/null +++ b/misc/tones/charge.txt @@ -0,0 +1 @@ +MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4
\ No newline at end of file diff --git a/misc/tones/cucuracha.txt b/misc/tones/cucuracha.txt new file mode 100644 index 000000000..dce328c03 --- /dev/null +++ b/misc/tones/cucuracha.txt @@ -0,0 +1 @@ +MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8
\ No newline at end of file diff --git a/misc/tones/daisy.txt b/misc/tones/daisy.txt new file mode 100644 index 000000000..e39410621 --- /dev/null +++ b/misc/tones/daisy.txt @@ -0,0 +1 @@ +MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8
\ No newline at end of file diff --git a/misc/tones/dixie.txt b/misc/tones/dixie.txt new file mode 100644 index 000000000..f3ddd19ed --- /dev/null +++ b/misc/tones/dixie.txt @@ -0,0 +1 @@ +MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16
\ No newline at end of file diff --git a/misc/tones/tell.txt b/misc/tones/tell.txt new file mode 100644 index 000000000..ea77d1a24 --- /dev/null +++ b/misc/tones/tell.txt @@ -0,0 +1 @@ +T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8 P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8 O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16 O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4 O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8 O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16 O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8 O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16 O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8 O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8 O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16 O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8 O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16 O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16 O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16 O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16 O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8 O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16 O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64 O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16 O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16 O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16 O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16 O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16 O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16 O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16 O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16 O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16 O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16 O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16 O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16 O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16 O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16 O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16 O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16 O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16 O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16 O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16 O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16 O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16 O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16 O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16 O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16 O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16 O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16 O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16 O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16 O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16 O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16 O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16 O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16 O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16 O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16 O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16 O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16 O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16 O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8 O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16 O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8 O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8 O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8 O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16 O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8 O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8 O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8 O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8 O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16 O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16 O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8 O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8 O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16 O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16 O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16 O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16 O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16 O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16 O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16 O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16 O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16 O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16 O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16 P16O4E16P16D+16P16C+16P16O3B16P16A+16P16 O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16 O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16 O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16 O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16 O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16 P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16 P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16 O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16 O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16 O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16 O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8 O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16 O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16 O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8 O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16 O4E16P16D+16P16C+16P16O3B16P16A+16P16A16 P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16 O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8 O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8 O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16 O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16 O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16 O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16 O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16 O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8 O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16 O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16 O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8 O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8 O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8 O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16 O2E2P64
\ No newline at end of file diff --git a/misc/tones/yankee.txt b/misc/tones/yankee.txt new file mode 100644 index 000000000..3677b39c1 --- /dev/null +++ b/misc/tones/yankee.txt @@ -0,0 +1 @@ +MNT150L8O2GGABGBADGGABL4GL8F+
\ No newline at end of file diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index 839631b3a..27ace4b7d 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -156,11 +156,6 @@ #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. @@ -216,33 +211,6 @@ #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. @@ -274,25 +242,6 @@ #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. @@ -316,27 +265,6 @@ #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 ************************************************************************************/ diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 5b91930c9..e43b9c18e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -327,7 +327,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_INTERRUPTSTACK=4096 # # Boot options @@ -368,7 +368,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=0 @@ -406,7 +406,7 @@ CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 CONFIG_NFILE_DESCRIPTORS=32 -CONFIG_NFILE_STREAMS=25 +CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 CONFIG_MQ_MAXMSGSIZE=32 @@ -476,11 +476,11 @@ 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_USART1_SERIAL_CONSOLE=y # 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 +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -509,8 +509,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # UART5 Configuration # -CONFIG_UART5_RXBUFSIZE=32 -CONFIG_UART5_TXBUFSIZE=32 +CONFIG_UART5_RXBUFSIZE=512 +CONFIG_UART5_TXBUFSIZE=512 CONFIG_UART5_BAUD=57600 CONFIG_UART5_BITS=8 CONFIG_UART5_PARITY=0 @@ -551,7 +551,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -565,8 +565,8 @@ 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_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" @@ -701,18 +701,18 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=4000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set # # Basic CXX Support # -# CONFIG_C99_BOOL8 is not set +CONFIG_C99_BOOL8=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y # CONFIG_CXX_NEWLONG is not set diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h new file mode 100755 index 000000000..507df70a2 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -0,0 +1,306 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> +#ifndef __ASSEMBLY__ +# include <stdint.h> +#endif + +#include <stm32.h> + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * 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_PPQ) + * 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 + * = (25,000,000 / 25) * 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_PCLK1_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_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) + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz + * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz + */ + +#ifdef CONFIG_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *  DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *  DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */ +#define GPIO_USART1_TX 0 /* USART1 is RX-only */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + * CAN2 is routed to the expansion connector. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 + +/* + * 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) + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#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_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 + +/************************************************************************************ + * 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); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h b/nuttx-configs/px4fmu-v2/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/px4fmu-v2/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-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs new file mode 100644 index 000000000..e70320aaa --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/px4fmu-v2/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-v2/nsh/appconfig b/nuttx-configs/px4fmu-v2/nsh/appconfig new file mode 100644 index 000000000..0e18aa8ef --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/appconfig @@ -0,0 +1,52 @@ +############################################################################ +# configs/px4fmu/nsh/appconfig +# +# 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline + +ifeq ($(CONFIG_CAN),y) +#CONFIGURED_APPS += examples/can +endif + +#ifeq ($(CONFIG_USBDEV),y) +#ifeq ($(CONFIG_CDCACM),y) +CONFIGURED_APPS += examples/cdcacm +#endif +#endif diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig new file mode 100644 index 000000000..372453fb6 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -0,0 +1,1060 @@ +# +# 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=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO 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 +# CONFIG_DEBUG_HARDFAULT 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 +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set + +# +# 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 is not set +# 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=y +# 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_STM32F427=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 is not set +CONFIG_STM32_OTGFS=y +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +CONFIG_STM32_SDIO=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +# CONFIG_STM32_SPI3 is not set +# CONFIG_STM32_SPI4 is not set +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +# CONFIG_STM32_TIM5 is not set +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_UART4=y +# CONFIG_STM32_UART5 is not set +CONFIG_STM32_USART6=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=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 is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +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_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_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_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_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_RS485 is not set +# CONFIG_USART3_RXDMA is not set +# CONFIG_UART4_RS485 is not set +# CONFIG_UART4_RXDMA is not set +# CONFIG_USART6_RS485 is not set +# CONFIG_USART6_RXDMA is not set +# CONFIG_UART7_RS485 is not set +# CONFIG_UART7_RXDMA is not set +# CONFIG_UART8_RS485 is not set +# CONFIG_UART8_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 + +# +# SDIO Configuration +# +CONFIG_SDIO_PRI=128 + +# +# 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=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# 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 + +# +# 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=y +# 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 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=8 +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=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +# CONFIG_MTD_PARTITION is not set +# CONFIG_MTD_BYTE_WRITE is not set + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 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_UART4=y +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_ARCH_HAVE_USART6=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART4_SERIAL_CONSOLE is not set +# CONFIG_USART6_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=115200 +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 + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y + +# +# UART4 Configuration +# +CONFIG_UART4_RXBUFSIZE=128 +CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_BITS=8 +CONFIG_UART4_PARITY=0 +CONFIG_UART4_2STOP=0 +# CONFIG_UART4_IFLOWCONTROL is not set +# CONFIG_UART4_OFLOWCONTROL is not set + +# +# USART6 Configuration +# +CONFIG_USART6_RXBUFSIZE=256 +CONFIG_USART6_TXBUFSIZE=256 +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 + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_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_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 is not set +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=512 +CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.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=y +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 is not set + +# +# 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 is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set + +# +# 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=32 +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=4000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +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_USBDEV_TRACE is not set +# 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 +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# 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-v2/nsh/setenv.sh b/nuttx-configs/px4fmu-v2/nsh/setenv.sh new file mode 100755 index 000000000..265520997 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/stm3240g-eval/nsh/setenv.sh +# +# 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. +# + +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 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 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" + +# This 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-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script new file mode 100644 index 000000000..1be39fb87 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/px4fmu/common/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 STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb 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 SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM 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 = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + 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-v2/src/Makefile b/nuttx-configs/px4fmu-v2/src/Makefile new file mode 100644 index 000000000..d4276f7fc --- /dev/null +++ b/nuttx-configs/px4fmu-v2/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/px4fmu/src/Makefile +# +# 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)/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-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4fmu-v2/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/board.h b/nuttx-configs/px4io-v1/include/board.h index 6503a94cd..815079266 100755 --- a/nuttx-configs/px4io-v1/include/board.h +++ b/nuttx-configs/px4io-v1/include/board.h @@ -115,22 +115,6 @@ #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 ************************************************************************************/ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h new file mode 100755 index 000000000..4b9c90638 --- /dev/null +++ b/nuttx-configs/px4io-v2/include/board.h @@ -0,0 +1,149 @@ +/************************************************************************************ + * 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> +#endif +#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 + +/************************************************************************************ + * 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-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs new file mode 100644 index 000000000..cd2d8eba3 --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -0,0 +1,170 @@ +############################################################################ +# configs/px4io-v2/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 + +# enable precise stack overflow tracking +#INSTRUMENTATIONDEFINES = -finstrument-functions \ +# -ffixed-r10 + +# 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-v2/nsh/appconfig b/nuttx-configs/px4io-v2/nsh/appconfig new file mode 100644 index 000000000..48a41bcdb --- /dev/null +++ b/nuttx-configs/px4io-v2/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-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig new file mode 100755 index 000000000..4111e70eb --- /dev/null +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -0,0 +1,548 @@ +############################################################################ +# configs/px4io/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 +CONFIG_ARCH_BOARD="px4io-v2" +CONFIG_ARCH_BOARD_PX4IO_V2=y +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 + +# +# 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=n +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 + +# +# PX4IO specific driver settings +# +# CONFIG_HRT_TIMER +# Enables the high-resolution timer. The board definition must +# set HRT_TIMER and HRT_TIMER_CHANNEL to the timer and capture/ +# compare channels to be used. +# CONFIG_HRT_PPM +# Enables R/C PPM input using the HRT. The board definition must +# set HRT_PPM_CHANNEL to the timer capture/compare channel to be +# used, and define GPIO_PPM_IN to configure the appropriate timer +# GPIO. +# CONFIG_PWM_SERVO +# Enables the PWM servo driver. The driver configuration must be +# supplied by the board support at initialisation time. +# Note that USART2 must be disabled on the PX4 board for this to +# be available. +# +# +CONFIG_HRT_TIMER=y +CONFIG_HRT_PPM=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= diff --git a/nuttx-configs/px4io-v2/nsh/setenv.sh b/nuttx-configs/px4io-v2/nsh/setenv.sh new file mode 100755 index 000000000..ff9a4bf8a --- /dev/null +++ b/nuttx-configs/px4io-v2/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-v2/scripts/ld.script b/nuttx-configs/px4io-v2/scripts/ld.script new file mode 100755 index 000000000..69c2f9cb2 --- /dev/null +++ b/nuttx-configs/px4io-v2/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-v2/src/Makefile b/nuttx-configs/px4io-v2/src/Makefile new file mode 100644 index 000000000..bb9539d16 --- /dev/null +++ b/nuttx-configs/px4io-v2/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-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/px4io-v2/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 index 5a8157deb..5e45cc936 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -77,10 +78,9 @@ 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), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), @@ -88,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _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")) + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = true; @@ -105,7 +104,12 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -118,20 +122,14 @@ Airspeed::init() 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; - + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); 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]); + differential_pressure_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); @@ -146,7 +144,14 @@ out: int Airspeed::probe() { - return measure(); + /* on initial power up the device needs more than one retry + for detection. Once it is running then retries aren't + needed + */ + _retries = 4; + int ret = measure(); + _retries = 2; + return ret; } int @@ -217,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) 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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -269,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t Airspeed::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct differential_pressure_s); + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -285,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(abuf)) { + ret += sizeof(*abuf); + abuf++; } } @@ -297,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -317,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(abuf)) { + ret = sizeof(*abuf); + } } while (0); @@ -330,7 +326,7 @@ Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -373,6 +369,12 @@ Airspeed::print_info() 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); + _reports->print_info("report queue"); +} + +void +Airspeed::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(&report)) + perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 89dfb22d7..048784813 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + protected: virtual int probe(); @@ -114,10 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; + uint16_t _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,7 +131,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; /** @@ -162,8 +163,12 @@ protected: */ static void cycle_trampoline(void *arg); + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); }; -/* 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/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 735bdb41a..b88f61ce8 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,8 +53,10 @@ #include <sys/prctl.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/safety.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> #include <systemlib/systemlib.h> @@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; + //XXX is this necessairy? armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); @@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index be8968a4e..01b89c8fa 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -41,6 +41,7 @@ #include <stdio.h> #include <fcntl.h> #include <unistd.h> +#include <math.h> #include <drivers/drv_gpio.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> @@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; - //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); - const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ + const float scaling = 510.0f; /**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ @@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float motor_calc[4] = {0}; float output_band = 0.0f; - float band_factor = 0.75f; const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ - float yaw_factor = 1.0f; static bool initialized = false; /* publish effective outputs */ static struct actuator_controls_effective_s actuator_controls_effective; static orb_advert_t actuator_controls_effective_pub; - if (motor_thrust <= min_thrust) { - motor_thrust = min_thrust; - output_band = 0.0f; - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { - output_band = band_factor * (motor_thrust - min_thrust); - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * startpoint_full_control; - } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { - output_band = band_factor * (max_thrust - motor_thrust); + /* linearly scale the control inputs from 0 to startpoint_full_control */ + if (motor_thrust < startpoint_full_control) { + output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + } else { + output_band = 1.0f; } + roll_control *= output_band; + pitch_control *= output_band; + yaw_control *= output_band; + + //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); - // if we are not in the output band - if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band - && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band - && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band - && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) { + /* if one motor is saturated, reduce throttle */ + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + + + if (saturation > 0.0f) { + + /* reduce the motor thrust according to the saturation */ + motor_thrust = motor_thrust - saturation; - yaw_factor = 0.5f; - yaw_control *= yaw_factor; // FRONT (MOTOR 1) motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control); - // RIGHT (MOTOR 2) motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control); - // BACK (MOTOR 3) motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control); - // LEFT (MOTOR 4) motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - for (int i = 0; i < 4; i++) { - //check for limits - if (motor_calc[i] < motor_thrust - output_band) { - motor_calc[i] = motor_thrust - output_band; - } - if (motor_calc[i] > motor_thrust + output_band) { - motor_calc[i] = motor_thrust + output_band; - } - } /* publish effective outputs */ actuator_controls_effective.control_effective[0] = roll_control; @@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ - /* scale up from 0..1 to 10..512) */ + /* scale up from 0..1 to 10..500) */ motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); - /* Keep motors spinning while armed and prevent overflows */ - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; - motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; - motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; - motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10; - - /* Failsafe logic - should never be necessary */ - motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511; - motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511; - motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511; - motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511; + /* scale up from 0..1 to 10..500) */ + motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); + + /* Failsafe logic for min values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; + motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas; + motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas; + motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas; + + /* Failsafe logic for max values - should never be necessary */ + motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas; + motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas; + motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas; + motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas; /* send motors via UART */ ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a5..2361f4dd1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -92,9 +92,6 @@ #include <nuttx/config.h> -#include <arch/board/board.h> -#include <drivers/device/i2c.h> - #include <sys/types.h> #include <stdint.h> #include <string.h> @@ -104,18 +101,23 @@ #include <unistd.h> #include <stdio.h> #include <ctype.h> - -#include <drivers/drv_blinkm.h> +#include <poll.h> #include <nuttx/wqueue.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> - #include <systemlib/systemlib.h> -#include <poll.h> + +#include <board_config.h> + +#include <drivers/device/i2c.h> +#include <drivers/drv_blinkm.h> + #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_gps_position.h> static const float MAX_CELL_VOLTAGE = 4.3f; @@ -375,7 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +404,12 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +487,28 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); + + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; + } else { + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -486,24 +524,24 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { + + for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) { if(vehicle_gps_position_raw.satellite_used[satloop] == 1) { num_of_used_sats++; } } - if(new_data_vehicle_status || no_data_vehicle_status < 3){ - if(num_of_cells == 0) { + if (new_data_vehicle_status || no_data_vehicle_status < 3) { + if (num_of_cells == 0) { /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf("<blinkm> cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -515,7 +553,7 @@ BlinkM::led() led_color_8 = LED_YELLOW; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -530,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -554,27 +592,24 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - case VEHICLE_FLIGHT_MODE_AUTO: + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) led_color_4 = LED_GREEN; - break; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; @@ -831,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 4409a8a9c..1590cc182 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -59,10 +59,11 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/device/spi.h> #include <drivers/drv_accel.h> +#include <drivers/device/ringbuffer.h> /* oddly, ERROR is not defined for c++ */ @@ -146,10 +147,7 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -233,16 +231,9 @@ private: int set_lowpass(unsigned frequency); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - - BMA180::BMA180(int bus, spi_dev_e device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -270,7 +261,7 @@ BMA180::~BMA180() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -286,16 +277,15 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -352,6 +342,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -367,10 +358,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(arp)) { + ret += sizeof(*arp); + arp++; } } @@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(arp)) + ret = sizeof(*arp); return ret; } @@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for 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 accel_report *buf = new struct accel_report[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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement */ @@ -654,7 +635,7 @@ BMA180::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); @@ -688,7 +669,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + struct accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -708,45 +689,41 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); + report.x_raw = (report.x_raw / 4); + report.y_raw = (report.y_raw / 4); + report.z_raw = (report.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; - - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; + report.y_raw = -report.y_raw; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.scaling = _accel_range_scale; + report.range_m_s2 = _accel_range_m_s2; - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); @@ -756,8 +733,7 @@ void BMA180::print_info() { perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu-v1/board_config.h index 383a046ff..27621211a 100644 --- a/src/drivers/boards/px4fmu/px4fmu_internal.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_internal.h + * @file board_config.h * * PX4FMU internal definitions */ @@ -51,13 +51,16 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include <stm32.h> - +#include <arch/board/board.h> /**************************************************************************************************** * Definitions ****************************************************************************************************/ /* Configuration ************************************************************************************/ +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS2" + //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" //#endif @@ -74,15 +77,47 @@ __BEGIN_DECLS /* External interrupts */ #define GPIO_EXTI_COMPASS (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) -// XXX MPU6000 DRDY? /* SPI chip selects */ - #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define GPIO_SPI_CS_ACCEL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +/* + * 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 + +/* + * I2C busses + */ +#define PX4_I2C_BUS_ESC 1 +#define PX4_I2C_BUS_ONBOARD 2 +#define PX4_I2C_BUS_EXPANSION 3 +#define PX4_I2C_BUS_LED 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_LED 0x55 + +#define PX4_I2C_OBDEV_PX4IO_BL 0x18 +#define PX4_I2C_OBDEV_PX4IO 0x1a + /* User GPIOs * * GPIO0-1 are the buffered high-power GPIOs. @@ -107,31 +142,45 @@ __BEGIN_DECLS #define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12) #define GPIO_GPIO_DIR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* + * 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) + +/* + * 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 + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) */ #define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) -/* PWM - * - * The PX4FMU has five PWM outputs, of which only the output on - * pin PC8 is fixed assigned to this function. The other four possible - * pwm sources are the TX, RX, RTS and CTS pins of USART2 - * - * Alternate function mapping: - * PC8 - BUZZER - TIM8_CH3/SDIO_D0 /TIM3_CH3/ USART6_CK / DCMI_D2 +/* High-resolution timer */ - -#ifdef CONFIG_PWM -# if defined(CONFIG_STM32_TIM3_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 3 -# elif defined(CONFIG_STM32_TIM8_PWM) -# define BUZZER_PWMCHANNEL 3 -# define BUZZER_PWMTIMER 8 -# endif -#endif +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ +#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) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b776917..66b776917 100644 --- a/src/drivers/boards/px4fmu/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk diff --git a/src/drivers/boards/px4fmu/px4fmu_can.c b/src/drivers/boards/px4fmu-v1/px4fmu_can.c index 187689ff9..1e1f10040 100644 --- a/src/drivers/boards/px4fmu/px4fmu_can.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_can.c @@ -54,7 +54,7 @@ #include "stm32.h" #include "stm32_can.h" -#include "px4fmu_internal.h" +#include "board_config.h" #ifdef CONFIG_CAN diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 36af2298c..4b12b75f9 100644 --- a/src/drivers/boards/px4fmu/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -59,7 +59,7 @@ #include <nuttx/analog/adc.h> #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include <arch/board/board.h> @@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void) * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects. * Keep the SPI2 init optional and conditionally initialize the ADC pins */ - spi2 = up_spiinitialize(2); - if (!spi2) { - message("[boot] Enabling IN12/13 instead of SPI2\n"); - /* no SPI2, use pins for ADC */ - stm32_configgpio(GPIO_ADC1_IN12); - stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - } else { + #ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); /* Default SPI2 to 1MHz and de-assert the known chip selects. */ SPI_SETFREQUENCY(spi2, 10000000); SPI_SETBITS(spi2, 8); @@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - } + #else + spi2 = NULL; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards + #endif /* Get the SPI port for the microSD slot */ diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 31b25984e..ea91f34ad 100644 --- a/src/drivers/boards/px4fmu/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -42,7 +42,7 @@ #include <stdbool.h> #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" #include <arch/board/board.h> @@ -57,6 +57,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS __EXPORT void led_init() @@ -94,3 +95,21 @@ __EXPORT void led_off(int led) stm32_gpiowrite(GPIO_LED2, true); } } + +__EXPORT void led_toggle(int led) +{ + if (led == 0) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + if (stm32_gpioread(GPIO_LED2)) + stm32_gpiowrite(GPIO_LED2, false); + else + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c index d85131dd8..848e21d79 100644 --- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_pwm_servo.c @@ -41,15 +41,15 @@ #include <stdint.h> -#include <drivers/stm32/drv_pwm_servo.h> - -#include <arch/board/board.h> -#include <drivers/drv_pwm_output.h> - #include <stm32.h> #include <stm32_gpio.h> #include <stm32_tim.h> +#include <drivers/stm32/drv_pwm_servo.h> +#include <drivers/drv_pwm_output.h> + +#include "board_config.h" + __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { .base = STM32_TIM2_BASE, diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index e05ddecf3..17e6862f7 100644 --- a/src/drivers/boards/px4fmu/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "chip.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Public Functions diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c index 0be981c1e..0fc8569aa 100644 --- a/src/drivers/boards/px4fmu/px4fmu_usb.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_usb.c @@ -53,7 +53,7 @@ #include "up_arch.h" #include "stm32.h" -#include "px4fmu_internal.h" +#include "board_config.h" /************************************************************************************ * Definitions diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h new file mode 100644 index 000000000..ec8dde499 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> +#include <stdint.h> + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include <stm32.h> +#include <arch/board/board.h> + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4IO connection configuration */ +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */ +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2 +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2 +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) + +/* External interrupts */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) + +/* SPI chip selects */ +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* 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_MAG 2 +#define PX4_SPIDEV_BARO 3 + +/* I2C busses */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED 2 + +/* Devices on the onboard bus. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_LED 0x55 +#define PX4_I2C_OBDEV_HMC5883 0x1e + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_VDD_SERVO_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/* PWM + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 +#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk new file mode 100644 index 000000000..99d37eeca --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -0,0 +1,10 @@ +# +# Board-specific startup code for the PX4FMUv2 +# + +SRCS = px4fmu_can.c \ + px4fmu2_init.c \ + px4fmu_pwm_servo.c \ + px4fmu_spi.c \ + px4fmu_usb.c \ + px4fmu2_led.c diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c new file mode 100644 index 000000000..ae2a645f7 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * 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 px4fmu_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/arch.h> +#include <nuttx/spi.h> +#include <nuttx/i2c.h> +#include <nuttx/sdio.h> +#include <nuttx/mmcsd.h> +#include <nuttx/analog/adc.h> +#include <nuttx/gran.h> + +#include <stm32.h> +#include "board_config.h" +#include <stm32_uart.h> + +#include <arch/board/board.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_led.h> + +#include <systemlib/cpuload.h> +#include <systemlib/perf_counter.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct sdio_dev_s *sdio; + +#include <math.h> + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ + stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ + stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ + + /* configure power supply control/sense pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_SERVO_VALID); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + + /* Configure SPI-based devices */ + + spi1 = up_spiinitialize(1); + + if (!spi1) { + message("[boot] FAILED to initialize SPI port 1\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + up_udelay(20); + + message("[boot] Successfully initialized SPI port 1\n"); + + /* Get the SPI port for the FRAM */ + + spi2 = up_spiinitialize(2); + + if (!spi2) { + message("[boot] FAILED to initialize SPI port 2\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + + /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 375000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH, false); + + message("[boot] Successfully initialized SPI port 2\n"); + + #ifdef CONFIG_MMCSD + /* First, get an instance of the SDIO interface */ + + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { + message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { + message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ + sdio_mediachange(sdio, true); + + message("[boot] Initialized SDIO\n"); + #endif + + return OK; +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c new file mode 100644 index 000000000..a856ccb02 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include <nuttx/config.h> + +#include <stdbool.h> + +#include "stm32.h" +#include "board_config.h" + +#include <arch/board/board.h> + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + /* Configure LED1 GPIO for output */ + + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void led_off(int led) +{ + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} + +__EXPORT void led_toggle(int led) +{ + if (led == 1) + { + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_can.c b/src/drivers/boards/px4fmu-v2/px4fmu_can.c new file mode 100644 index 000000000..f66c7cd79 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_can.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <errno.h> +#include <debug.h> + +#include <nuttx/can.h> +#include <arch/board/board.h> + +#include "chip.h" +#include "up_arch.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/* Debug ***************************************************************************/ +/* Non-standard debug that may be enabled just for testing CAN */ + +#ifdef CONFIG_DEBUG_CAN +# define candbg dbg +# define canvdbg vdbg +# define canlldbg lldbg +# define canllvdbg llvdbg +#else +# define candbg(x...) +# define canvdbg(x...) +# define canlldbg(x...) +# define canllvdbg(x...) +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + candbg("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + candbg("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif
\ No newline at end of file diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c new file mode 100644 index 000000000..600dfef41 --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_pwm_servo.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include <stdint.h> + +#include <stm32.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +#include <drivers/stm32/drv_pwm_servo.h> +#include <drivers/drv_pwm_output.h> + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + } +}; diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c new file mode 100644 index 000000000..09838d02f --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * 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 px4fmu_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <nuttx/spi.h> +#include <arch/board/board.h> + +#include <up_arch.h> +#include <chip.h> +#include <stm32.h> +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI_CS_FRAM); + stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); +#endif +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_usb.c b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c new file mode 100644 index 000000000..f329e06ff --- /dev/null +++ b/src/drivers/boards/px4fmu-v2/px4fmu_usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <nuttx/usb/usbdev.h> +#include <nuttx/usb/usbdev_trace.h> + +#include <up_arch.h> +#include <stm32.h> +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + ulldbg("resume: %d\n", resume); +} + diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io-v1/board_config.h index 6638e715e..48aadbd76 100644 --- a/src/drivers/boards/px4io/px4io_internal.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4io_internal.h + * @file board_config.h * * PX4IO hardware definitions. */ @@ -47,7 +47,9 @@ #include <nuttx/compiler.h> #include <stdint.h> +/* these headers are not C++ safe */ #include <stm32.h> +#include <arch/board/board.h> /************************************************************************************ * Definitions @@ -83,3 +85,11 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN diff --git a/src/drivers/boards/px4io/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15..2601a1c15 100644 --- a/src/drivers/boards/px4io/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 15c59e423..8292da9e1 100644 --- a/src/drivers/boards/px4io/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -55,7 +55,7 @@ #include <nuttx/arch.h> #include "stm32.h" -#include "px4io_internal.h" +#include "board_config.h" #include "stm32_uart.h" #include <arch/board/board.h> diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c index 6df470da6..6df470da6 100644 --- a/src/drivers/boards/px4io/px4io_pwm_servo.c +++ b/src/drivers/boards/px4io-v1/px4io_pwm_servo.c diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h new file mode 100644 index 000000000..4d41d0d07 --- /dev/null +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 px4iov2_internal.h + * + * PX4IOV2 internal definitions + */ + +#pragma once + +/****************************************************************************** + * Included Files + ******************************************************************************/ + +#include <nuttx/config.h> +#include <nuttx/compiler.h> +#include <stdint.h> + +/* these headers are not C++ safe */ +#include <stm32.h> +#include <arch/board/board.h> + +/****************************************************************************** + * Definitions + ******************************************************************************/ +/* Configuration **************************************************************/ + +/****************************************************************************** + * Serial + ******************************************************************************/ +#define PX4FMU_SERIAL_BASE STM32_USART2_BASE +#define PX4FMU_SERIAL_VECTOR STM32_IRQ_USART2 +#define PX4FMU_SERIAL_TX_GPIO GPIO_USART2_TX +#define PX4FMU_SERIAL_RX_GPIO GPIO_USART2_RX +#define PX4FMU_SERIAL_TX_DMA DMACHAN_USART2_TX +#define PX4FMU_SERIAL_RX_DMA DMACHAN_USART2_RX +#define PX4FMU_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4FMU_SERIAL_BITRATE 1500000 + +/****************************************************************************** + * GPIOS + ******************************************************************************/ + +/* LEDS **********************************************************************/ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) + +/* Safety switch button *******************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ******************************************************/ + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) + +/* Analog inputs **************************************************************/ + +#define GPIO_ADC_VSERVO (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) + +/* the same rssi signal goes to both an adc and a timer input */ +#define GPIO_ADC_RSSI (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) +#define GPIO_TIM_RSSI (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN12) + +/* PWM pins **************************************************************/ + +#define GPIO_PPM (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN8) + +#define GPIO_PWM1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_PWM2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_PWM3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_PWM4 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_PWM5 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_PWM6 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_PWM7 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* SBUS pins *************************************************************/ + +/* XXX these should be UART pins */ +#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) +#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) + +/* + * High-resolution timer + */ +#define HRT_TIMER 1 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ +#define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN GPIO_TIM1_CH1IN + +/* LED definitions ******************************************************************/ +/* PX4 has two LEDs that we will encode as: */ + +#define LED_STARTED 0 /* LED? */ +#define LED_HEAPALLOCATE 1 /* LED? */ +#define LED_IRQSENABLED 2 /* LED? + LED? */ +#define LED_STACKCREATED 3 /* LED? */ +#define LED_INIRQ 4 /* LED? + LED? */ +#define LED_SIGNAL 5 /* LED? + LED? */ +#define LED_ASSERTION 6 /* LED? + LED? + LED? */ +#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk new file mode 100644 index 000000000..85f94e8be --- /dev/null +++ b/src/drivers/boards/px4io-v2/module.mk @@ -0,0 +1,6 @@ +# +# Board-specific startup code for the PX4IOv2 +# + +SRCS = px4iov2_init.c \ + px4iov2_pwm_servo.c diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c new file mode 100644 index 000000000..ccd01edf5 --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -0,0 +1,160 @@ +/**************************************************************************** + * + * 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 px4iov2_init.c + * + * PX4FMU-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/arch.h> + +#include <stm32.h> +#include "board_config.h" + +#include <arch/board/board.h> + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + + /* configure GPIOs */ + + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + + stm32_configgpio(GPIO_BTN_SAFETY); + + /* spektrum power enable is active high - enable it by default */ + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_SERVO_FAULT_DETECT); + + /* RSSI inputs */ + stm32_configgpio(GPIO_TIM_RSSI); /* xxx alternate function */ + stm32_configgpio(GPIO_ADC_RSSI); + + /* servo rail voltage */ + stm32_configgpio(GPIO_ADC_VSERVO); + + stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); + stm32_configgpio(GPIO_SBUS_OUTPUT); + + /* sbus output enable is active low - disable it by default */ + stm32_gpiowrite(GPIO_SBUS_OENABLE, true); + stm32_configgpio(GPIO_SBUS_OENABLE); + + stm32_configgpio(GPIO_PPM); /* xxx alternate function */ + + stm32_gpiowrite(GPIO_PWM1, false); + stm32_configgpio(GPIO_PWM1); + + stm32_gpiowrite(GPIO_PWM2, false); + stm32_configgpio(GPIO_PWM2); + + stm32_gpiowrite(GPIO_PWM3, false); + stm32_configgpio(GPIO_PWM3); + + stm32_gpiowrite(GPIO_PWM4, false); + stm32_configgpio(GPIO_PWM4); + + stm32_gpiowrite(GPIO_PWM5, false); + stm32_configgpio(GPIO_PWM5); + + stm32_gpiowrite(GPIO_PWM6, false); + stm32_configgpio(GPIO_PWM6); + + stm32_gpiowrite(GPIO_PWM7, false); + stm32_configgpio(GPIO_PWM7); + + stm32_gpiowrite(GPIO_PWM8, false); + stm32_configgpio(GPIO_PWM8); +} diff --git a/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c new file mode 100644 index 000000000..4f1b9487c --- /dev/null +++ b/src/drivers/boards/px4io-v2/px4iov2_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4iov2_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include <stdint.h> + +#include <drivers/stm32/drv_pwm_servo.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include <stm32.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052..a9e22eaa6 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,7 +1,6 @@ /**************************************************************************** * * 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 @@ -35,15 +34,14 @@ /** * @file ringbuffer.h * - * A simple ringbuffer template. + * A flexible ringbuffer class. */ #pragma once -template<typename T> class RingBuffer { public: - RingBuffer(unsigned size); + RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); /** @@ -52,15 +50,37 @@ public: * @param val Item to put * @return true if the item was put, false if the buffer is full */ - bool put(T &val); + bool put(const void *val, size_t val_size = 0); + + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); /** - * Put an item into the buffer. + * Force an item into the buffer, discarding an older item if there is not space. * * @param val Item to put - * @return true if the item was put, false if the buffer is full + * @return true if an item was discarded to make space */ - bool put(const T &val); + bool force(const void *val, size_t val_size = 0); + + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); /** * Get an item from the buffer. @@ -68,15 +88,18 @@ public: * @param val Item that was gotten * @return true if an item was got, false if the buffer was empty. */ - bool get(T &val); + bool get(void *val, size_t val_size = 0); - /** - * Get an item from the buffer (scalars only). - * - * @return The value that was fetched, or zero if the buffer was - * empty. - */ - T get(void); + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); /* * Get the number of slots free in the buffer. @@ -97,54 +120,103 @@ public: /* * Returns true if the buffer is empty. */ - bool empty() { return _tail == _head; } + bool empty(); /* * Returns true if the buffer is full. */ - bool full() { return _next(_head) == _tail; } + bool full(); /* * Returns the capacity of the buffer, or zero if the buffer could * not be allocated. */ - unsigned size() { return (_buf != nullptr) ? _size : 0; } + unsigned size(); /* * Empties the buffer. */ - void flush() { _head = _tail = _size; } + void flush(); + + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); private: - T *const _buf; - const unsigned _size; - volatile unsigned _head; /**< insertion point */ - volatile unsigned _tail; /**< removal point */ + unsigned _num_items; + const size_t _item_size; + char *_buf; + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ 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) +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), + _item_size(item_size), + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) {} -template <typename T> -RingBuffer<T>::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template <typename T> -bool RingBuffer<T>::put(T &val) +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _num_items : (index - 1); +} + +bool +RingBuffer::empty() +{ + return _tail == _head; +} + +bool +RingBuffer::full() +{ + return _next(_head) == _tail; +} + +unsigned +RingBuffer::size() +{ + return (_buf != nullptr) ? _num_items : 0; +} + +void +RingBuffer::flush() +{ + while (!empty()) + get(NULL); +} + +bool +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { - _buf[_head] = val; + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; } else { @@ -152,52 +224,286 @@ bool RingBuffer<T>::put(T &val) } } -template <typename T> -bool RingBuffer<T>::put(const T &val) +bool +RingBuffer::put(int8_t val) { - unsigned next = _next(_head); - if (next != _tail) { - _buf[_head] = val; - _head = next; - return true; - } else { - return false; + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) +{ + bool overwrote = false; + + for (;;) { + if (put(val, val_size)) + break; + get(NULL); + overwrote = true; } + return overwrote; +} + +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); } -template <typename T> -bool RingBuffer<T>::get(T &val) +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { - val = _buf[_tail]; - _tail = _next(_tail); + unsigned candidate; + unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + if (val != NULL) + memcpy(val, &_buf[candidate * _item_size], val_size); + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + return true; } else { return false; } } -template <typename T> -T RingBuffer<T>::get(void) +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::space(void) +bool +RingBuffer::get(int16_t &val) { - return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::count(void) +bool +RingBuffer::get(uint16_t &val) { - return _size - space(); + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space(void) +{ + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); +} + +unsigned +RingBuffer::count(void) +{ + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ + return _num_items - space(); +} + +bool +RingBuffer::resize(unsigned new_size) +{ + char *old_buffer; + char *new_buffer = new char [(new_size+1) * _item_size]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _num_items = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; } -template <typename T> -unsigned RingBuffer<T>::_next(unsigned index) +void +RingBuffer::print_info(const char *name) { - return (0 == index) ? _size : (index - 1); + printf("%s %u/%u (%u/%u @ %p)\n", + name, + _num_items, + _num_items * _item_size, + _head, + _tail, + _buf); } diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 8fffd60cb..fa6b78d64 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -67,6 +67,7 @@ SPI::SPI(const char *name, CDev(name, devname, irq), // public // protected + locking_mode(LOCK_PREEMPTION), // private _bus(bus), _device(device), @@ -132,13 +133,25 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + irqstate_t state; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; - /* do common setup */ - if (!up_interrupt_context()) - SPI_LOCK(_dev, true); + /* lock the bus as required */ + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + state = irqsave(); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + break; + case LOCK_NONE: + break; + } + } SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); @@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) - SPI_LOCK(_dev, false); + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + irqrestore(state); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + break; + } + } return OK; } diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index e0122372a..9103dca2e 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + LockMode locking_mode; /**< selected locking mode */ + private: int _bus; enum spi_dev_e _device; diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 794de584b..7d93ed938 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Accelerometer driver interface. + * @file drv_accel.h + * + * Accelerometer driver interface. */ #ifndef _DRV_ACCEL_H @@ -52,6 +54,7 @@ */ struct accel_report { uint64_t timestamp; + uint64_t error_count; float x; /**< acceleration in the NED X board axis in m/s^2 */ float y; /**< acceleration in the NED Y board axis in m/s^2 */ float z; /**< acceleration in the NED Z board axis in m/s^2 */ @@ -65,7 +68,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 7bb9ee2af..78f31495d 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -32,7 +32,10 @@ ****************************************************************************/ /** - * @file Airspeed driver interface. + * @file drv_airspeed.h + * + * Airspeed driver interface. + * * @author Simon Wilks */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 176f798c0..e2f0137ae 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Barometric pressure sensor driver interface. + * @file drv_baro.h + * + * Barometric pressure sensor driver interface. */ #ifndef _DRV_BARO_H @@ -55,6 +57,7 @@ struct baro_report { float altitude; float temperature; uint64_t timestamp; + uint64_t error_count; }; /* diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 510983d4b..37af26d52 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * 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 @@ -59,17 +59,47 @@ # define GPIO_CAN_RX (1<<7) /**< CAN2 RX */ /** - * 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. + * Device paths for things that support the GPIO ioctl protocol. */ # define PX4FMU_DEVICE_PATH "/dev/px4fmu" # define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifndef PX4IO_DEVICE_PATH -# error No GPIO support for this board. +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/* + * PX4FMUv2 GPIO numbers. + * + * There are no alternate functions on this board. + */ +# define GPIO_SERVO_1 (1<<0) /**< servo 1 output */ +# define GPIO_SERVO_2 (1<<1) /**< servo 2 output */ +# define GPIO_SERVO_3 (1<<2) /**< servo 3 output */ +# define GPIO_SERVO_4 (1<<3) /**< servo 4 output */ +# define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ +# define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ + +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - !VDD_5V_PERIPH_EN */ +# define GPIO_3V3_SENSORS_EN (1<<7) /**< PE3 - VDD_3V3_SENSORS_EN */ +# define GPIO_BRICK_VALID (1<<8) /**< PB5 - !VDD_BRICK_VALID */ +# define GPIO_SERVO_VALID (1<<9) /**< PB7 - !VDD_SERVO_VALID */ +# define GPIO_5V_HIPOWER_OC (1<<10) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<11) /**< PE10 - !VDD_5V_PERIPH_OC */ + +/** + * Device paths for things that support the GPIO ioctl protocol. + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" + +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 +/* no GPIO driver on the PX4IOv1 board */ +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 +/* no GPIO driver on the PX4IOv2 board */ #endif /* diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 1dda8ef0b..06e3535b3 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file GPS driver interface. + * @file drv_gps.h + * + * GPS driver interface. */ #ifndef _DRV_GPS_H @@ -51,8 +53,7 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_NMEA, + GPS_DRIVER_MODE_MTK } gps_driver_mode_t; diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 1d0fef6fc..2ae8c7058 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Gyroscope driver interface. + * @file drv_gyro.h + * + * Gyroscope driver interface. */ #ifndef _DRV_GYRO_H @@ -52,6 +54,7 @@ */ struct gyro_report { uint64_t timestamp; + uint64_t error_count; float x; /**< angular velocity in the NED X board axis in rad/s */ float y; /**< angular velocity in the NED Y board axis in rad/s */ float z; /**< angular velocity in the NED Z board axis in rad/s */ diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index 21044f620..4ce04696e 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -54,12 +54,13 @@ #define LED_ON _IOC(_LED_BASE, 0) #define LED_OFF _IOC(_LED_BASE, 1) +#define LED_TOGGLE _IOC(_LED_BASE, 2) __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(void); +__EXPORT void drv_led_start(void); __END_DECLS diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a1..06107bd3d 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -54,6 +54,7 @@ */ struct mag_report { uint64_t timestamp; + uint64_t error_count; float x; float y; float z; @@ -90,25 +91,37 @@ ORB_DECLARE(sensor_mag); /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) +/** return the mag internal sample rate in Hz */ +#define MAGIOCGSAMPLERATE _MAGIOC(1) + /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(1) +#define MAGIOCSLOWPASS _MAGIOC(2) + +/** return the mag internal lowpass filter in Hz */ +#define MAGIOCGLOWPASS _MAGIOC(3) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(2) +#define MAGIOCSSCALE _MAGIOC(4) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(3) +#define MAGIOCGSCALE _MAGIOC(5) /** set the measurement range to handle (at least) arg Gauss */ -#define MAGIOCSRANGE _MAGIOC(4) +#define MAGIOCSRANGE _MAGIOC(6) + +/** return the current mag measurement range in Gauss */ +#define MAGIOCGRANGE _MAGIOC(7) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(5) +#define MAGIOCCALIBRATE _MAGIOC(8) /** excite strap */ -#define MAGIOCEXSTRAP _MAGIOC(6) +#define MAGIOCEXSTRAP _MAGIOC(9) /** perform self test and report status */ -#define MAGIOCSELFTEST _MAGIOC(7) +#define MAGIOCSELFTEST _MAGIOC(10) + +/** determine if mag is external or onboard */ +#define MAGIOCGEXTERNAL _MAGIOC(11) #endif /* _DRV_MAG_H */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca09..51f916f37 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,6 +65,36 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** + * Lowest minimum PWM in us + */ +#define PWM_LOWEST_MIN 900 + +/** + * Default minimum PWM in us + */ +#define PWM_DEFAULT_MIN 1000 + +/** + * Highest PWM allowed as the minimum PWM + */ +#define PWM_HIGHEST_MIN 1300 + +/** + * Highest maximum PWM in us + */ +#define PWM_HIGHEST_MAX 2100 + +/** + * Default maximum PWM in us + */ +#define PWM_DEFAULT_MAX 2000 + +/** + * Lowest PWM allowed as the maximum PWM + */ +#define PWM_LOWEST_MAX 1700 + +/** * Servo output signal type, value is actual servo output pulse * width in microseconds. */ @@ -79,6 +109,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; + unsigned channel_count; }; /* @@ -100,26 +131,63 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +/** get default servo update rate */ +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + /** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3) + +/** get alternate servo update rate */ +#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5) /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6) + +/** check the selected update rates */ +#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7) /** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5) +#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8) /** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9) /** start DSM bind */ -#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10) + +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ + +/** power up DSM receiver */ +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) + +/** set the PWM value for failsafe */ +#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value for failsafe */ +#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13) + +/** set the PWM value when disarmed - should be no PWM (zero) by default */ +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14) + +/** get the PWM value when disarmed */ +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15) + +/** set the minimum PWM value the output will send */ +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16) + +/** get the minimum PWM value the output will send */ +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17) + +/** set the maximum PWM value the output will send */ +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18) -/** Power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +/** get the maximum PWM value the output will send */ +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 03a82ec5d..cf91f7030 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -52,6 +52,7 @@ */ struct range_finder_report { uint64_t timestamp; + uint64_t error_count; float distance; /** in meters */ uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ }; diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 4decc324e..78ffad9d7 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file R/C input interface. + * @file drv_rc_input.h * + * R/C input interface. */ #ifndef _DRV_RC_INPUT_H diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h new file mode 100644 index 000000000..07c6186dd --- /dev/null +++ b/src/drivers/drv_rgbled.h @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file drv_rgbled.h + * + * RGB led device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +/* more devices will be 1, 2, etc */ +#define RGBLED_DEVICE_PATH "/dev/rgbled0" + +/* + * ioctl() definitions + */ + +#define _RGBLEDIOCBASE (0x2900) +#define _RGBLEDIOC(_n) (_IOC(_RGBLEDIOCBASE, _n)) + +/** play the named script in *(char *)arg, repeating forever */ +#define RGBLED_PLAY_SCRIPT_NAMED _RGBLEDIOC(1) + +/** play the numbered script in (arg), repeating forever */ +#define RGBLED_PLAY_SCRIPT _RGBLEDIOC(2) + +/** + * Set the user script; (arg) is a pointer to an array of script lines, + * where each line is an array of four bytes giving <duration>, <command>, arg[0-2] + * + * The script is terminated by a zero command. + */ +#define RGBLED_SET_USER_SCRIPT _RGBLEDIOC(3) + +/** set constant RGB values */ +#define RGBLED_SET_RGB _RGBLEDIOC(4) + +/** set color */ +#define RGBLED_SET_COLOR _RGBLEDIOC(5) + +/** set blink speed */ +#define RGBLED_SET_MODE _RGBLEDIOC(6) + +/** set pattern */ +#define RGBLED_SET_PATTERN _RGBLEDIOC(7) + + +/* + structure passed to RGBLED_SET_RGB ioctl() + Note that the driver scales the brightness to 0 to 255, regardless + of the hardware scaling + */ +typedef struct { + uint8_t red; + uint8_t green; + uint8_t blue; +} rgbled_rgbset_t; + +/* enum passed to RGBLED_SET_COLOR ioctl()*/ +typedef enum { + RGBLED_COLOR_OFF, + RGBLED_COLOR_RED, + RGBLED_COLOR_YELLOW, + RGBLED_COLOR_PURPLE, + RGBLED_COLOR_GREEN, + RGBLED_COLOR_BLUE, + RGBLED_COLOR_WHITE, + RGBLED_COLOR_AMBER, + RGBLED_COLOR_DIM_RED, + RGBLED_COLOR_DIM_YELLOW, + RGBLED_COLOR_DIM_PURPLE, + RGBLED_COLOR_DIM_GREEN, + RGBLED_COLOR_DIM_BLUE, + RGBLED_COLOR_DIM_WHITE, + RGBLED_COLOR_DIM_AMBER +} rgbled_color_t; + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +typedef enum { + RGBLED_MODE_OFF, + RGBLED_MODE_ON, + RGBLED_MODE_BLINK_SLOW, + RGBLED_MODE_BLINK_NORMAL, + RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_BREATHE, + RGBLED_MODE_PATTERN +} rgbled_mode_t; + +#define RGBLED_PATTERN_LENGTH 20 + +typedef struct { + rgbled_color_t color[RGBLED_PATTERN_LENGTH]; + unsigned duration[RGBLED_PATTERN_LENGTH]; +} rgbled_pattern_t; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 3a89cab9d..8e8b2c1b9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Common sensor API and ioctl definitions. + * @file drv_sensor.h + * + * Common sensor API and ioctl definitions. */ #ifndef _DRV_SENSOR_H diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 0c6afc64c..cb5fd53a5 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -31,7 +31,9 @@ * ****************************************************************************/ -/* +/** + * @file drv_tone_alarm.h + * * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" @@ -60,6 +62,8 @@ #include <sys/ioctl.h> +#define TONEALARM_DEVICE_PATH "/dev/tone_alarm" + #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) @@ -125,4 +129,25 @@ enum tone_pitch { TONE_NOTE_MAX }; +enum { + TONE_STOP_TUNE = 0, + TONE_STARTUP_TUNE, + TONE_ERROR_TUNE, + TONE_NOTIFY_POSITIVE_TUNE, + TONE_NOTIFY_NEUTRAL_TUNE, + TONE_NOTIFY_NEGATIVE_TUNE, + /* Do not include these unused tunes + TONE_CHARGE_TUNE, + TONE_DIXIE_TUNE, + TONE_CUCURACHA_TUNE, + TONE_YANKEE_TUNE, + TONE_DAISY_TUNE, + TONE_WILLIAM_TELL_TUNE, */ + TONE_ARMING_WARNING_TUNE, + TONE_BATTERY_WARNING_SLOW_TUNE, + TONE_BATTERY_WARNING_FAST_TUNE, + TONE_GPS_WARNING_TUNE, + TONE_NUMBER_OF_TUNES +}; + #endif /* DRV_TONE_ALARM_H_ */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index cd72d9d23..9d8ad084e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -59,7 +59,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/airspeed.h> #include <systemlib/err.h> @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -131,11 +132,8 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } @@ -152,48 +150,44 @@ ETSAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + perf_count(_comms_errors); return ret; } 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; + // 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 + perf_count(_comms_errors); + log("zero value from sensor"); + return -1; } if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; - } else { diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].differential_pressure_pa = diff_pres_pa; - // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + // XXX we may want to smooth out the readings to remove noise. + differential_pressure_s report; + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.differential_pressure_pa = diff_pres_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -213,7 +207,7 @@ ETSAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + perf_count(_comms_errors); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 38835418b..fc500a9ec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char* uart_path); + GPS(const char *uart_path); virtual ~GPS(); virtual int init(); @@ -156,7 +156,7 @@ GPS *g_dev; } -GPS::GPS(const char* uart_path) : +GPS::GPS(const char *uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -192,6 +192,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); + g_dev = nullptr; } @@ -270,19 +271,20 @@ GPS::task_main() } switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; } + unlock(); + if (_Helper->configure(_baudrate) == 0) { unlock(); @@ -294,6 +296,7 @@ GPS::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } @@ -310,10 +313,26 @@ GPS::task_main() } if (!_healthy) { - warnx("module found"); + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); _healthy = true; } } + if (_healthy) { warnx("module lost"); _healthy = false; @@ -322,25 +341,26 @@ GPS::task_main() lock(); } + lock(); /* select next mode */ switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + + default: + break; } } - debug("exiting"); + + warnx("exiting"); ::close(_serial_fd); @@ -361,23 +381,25 @@ void GPS::print_info() { switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; + + default: + break; } + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + if (_report.timestamp_position != 0) { - warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, + _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); @@ -428,6 +450,7 @@ start(const char *uart_path) errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } + exit(0); fail: @@ -503,7 +526,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = GPS_DEFAULT_UART_PORT; + char *device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. @@ -513,15 +536,18 @@ gps_main(int argc, char *argv[]) if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; + } else { goto out; } } + gps::start(device_name); } if (!strcmp(argv[1], "stop")) gps::stop(); + /* * Test the driver/device. */ diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index ba86d370a..2e2cbc8dd 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); + warnx("try baudrate: %d\n", speed); default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } + struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); return -1; } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index defc1a074..73d4b889c 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -33,7 +33,7 @@ * ****************************************************************************/ -/** +/** * @file gps_helper.h */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 62941d74b..56b702ea6 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -48,9 +48,9 @@ MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_mtk_revision(0) + _fd(fd), + _gps_position(gps_position), + _mtk_revision(0) { decode_init(); } @@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate) warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { @@ -128,12 +132,15 @@ MTK::receive(unsigned timeout) handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } + j++; } + /* everything is read */ j = count = 0; } @@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 19; @@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) add_byte_to_checksum(b); // Fill packet buffer - ((uint8_t*)(&packet))[_rx_count] = b; + ((uint8_t *)(&packet))[_rx_count] = b; _rx_count++; /* Packet size minus checksum, XXX ? */ @@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) /* Compare checksum */ if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { ret = 1; + } else { warnx("MTK Checksum invalid"); ret = -1; } + // Reset state machine to decode next packet decode_init(); } } + return ret; } @@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet) if (_mtk_revision == 16) { _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { _gps_position->lat = packet.latitude; // both degrees*1e7 _gps_position->lon = packet.longitude; // both degrees*1e7 + } else { warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; } + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit _gps_position->epv_m = 0.0; //unknown in mtk custom mode - _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b579db715..86291901c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -42,13 +42,14 @@ * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ - -#include <unistd.h> -#include <stdio.h> -#include <poll.h> +#include <assert.h> #include <math.h> +#include <poll.h> +#include <stdio.h> #include <string.h> -#include <assert.h> +#include <time.h> +#include <unistd.h> + #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_gps_position.h> @@ -59,13 +60,14 @@ #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 +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), _configured(false), _waiting_for_ack(false), - _disable_cmd_counter(0) + _disable_cmd_last(0) { decode_init(); } @@ -190,35 +192,35 @@ UBX::configure(unsigned &baudrate) 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"); + warnx("ubx: msg rate configuration failed: NAV POSLLH"); 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"); + warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); 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"); + warnx("ubx: msg rate configuration failed: NAV SOL"); 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"); + warnx("ubx: msg rate configuration failed: NAV VELNED"); 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"); + warnx("ubx: msg rate configuration failed: NAV SVINFO"); return 1; } @@ -270,11 +272,17 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ + warnx("ubx: poll error"); return -1; } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - return handled ? 1 : -1; + if (handled) { + return 1; + + } else { + return -1; + } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -291,8 +299,6 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; 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() > 0) handled = true; } @@ -302,6 +308,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { + warnx("ubx: timeout - no useful messages"); return -1; } } @@ -453,6 +460,15 @@ UBX::handle_message() timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME, &ts); +#endif + _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(); @@ -554,10 +570,13 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + + hrt_abstime t = hrt_absolute_time(); - if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } @@ -630,7 +649,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length) ck_b = ck_b + ck_a; } - /* The checksum is written to the last to bytes of a message */ + /* the checksum is written to the last to bytes of a message */ message[length - 2] = ck_a; message[length - 1] = ck_b; } @@ -659,17 +678,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; - /* Calculate the checksum now */ + /* calculate the checksum now */ add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; - /* Start with the two sync bytes */ + /* start with the two sync bytes */ ret += write(fd, sync_bytes, sizeof(sync_bytes)); ret += write(fd, packet, length); if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: config write fail"); + warnx("ubx: configuration write fail"); } void @@ -686,7 +705,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) 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 + /* configure ACK check */ _message_class_needed = msg_class; _message_id_needed = msg_id; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 4fc276975..76ef873a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -347,7 +347,7 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t *message, const unsigned length); /** * Helper to send a config packet @@ -358,7 +358,7 @@ private: void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); int wait_for_ack(unsigned timeout); @@ -376,7 +376,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_counter; + uint8_t _disable_cmd_last; }; #endif /* UBX_H_ */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index bd027ce0b..c1d73dd87 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> @@ -403,7 +404,7 @@ HIL::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (i < (unsigned)outputs.noutputs && + if (i < outputs.noutputs && isfinite(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { @@ -516,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) g_hil->set_pwm_rate(arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: // HIL always outputs at the alternate (usually faster) rate break; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ac3bdc132..5f0ce4ff8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -58,13 +58,14 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -77,8 +78,8 @@ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 -/* Max measurement rate is 160Hz */ -#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */ +/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ +#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ #define ADDR_CONF_A 0x00 #define ADDR_CONF_B 0x01 @@ -148,10 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -167,6 +165,8 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ + int _bus; /**< the bus the device is connected to */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -190,6 +190,11 @@ private: void stop(); /** + * Reset the device + */ + int reset(); + + /** * Perform the on-sensor scale calibration routine. * * @note The sensor will continue to provide measurements, these @@ -303,9 +308,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - /* * Driver 'main' command. */ @@ -315,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), @@ -326,7 +325,8 @@ HMC5883::HMC5883(int bus) : _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), - _calibrated(false) + _calibrated(false), + _bus(bus) { // enable debug() calls _debug_enabled = false; @@ -348,9 +348,13 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); } int @@ -363,24 +367,21 @@ HMC5883::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct mag_report[_num_reports]; - + _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; + /* reset the device configuration */ + reset(); /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); - /* set range */ - set_range(_range_ga); - ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; @@ -480,6 +481,7 @@ ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -488,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* 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 (_reports->get(mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; } } @@ -509,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -526,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } } while (0); return ret; @@ -539,108 +538,105 @@ int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: - return -EINVAL; + /* 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 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(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; + } - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* 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); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) + return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) return SENSOR_POLLRATE_MANUAL; - return (1000 / _measure_ticks); + return 1000000/TICK2USEC(_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)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: - /* XXX implement this */ - return -EINVAL; + return reset(); case MAGIOCSSAMPLERATE: - /* not supported, always 1 sample per poll */ - return -EINVAL; + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + + case MAGIOCGSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return 1000000/TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; @@ -665,6 +661,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return check_calibration(); + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) + return 1; + else + return 0; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -676,7 +678,7 @@ HMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); @@ -688,6 +690,13 @@ HMC5883::stop() work_cancel(HPWORK, &_work); } +int +HMC5883::reset() +{ + /* set range */ + return set_range(_range_ga); +} + void HMC5883::cycle_trampoline(void *arg) { @@ -778,9 +787,11 @@ HMC5883::collect() perf_begin(_sample_perf); + struct mag_report new_report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that @@ -810,8 +821,10 @@ HMC5883::collect() */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || - (abs(report.z) > 2048)) + (abs(report.z) > 2048)) { + perf_count(_comms_errors); goto out; + } /* * RAW outputs @@ -819,10 +832,10 @@ HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + new_report.x_raw = report.y; + new_report.y_raw = -report.x; /* z remains z */ - _reports[_next_report].z_raw = report.z; + new_report.z_raw = report.z; /* scale values for output */ @@ -844,33 +857,30 @@ HMC5883::collect() #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* XXX axis assignment of external sensor is yet unknown */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, + * therefore switch x and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + /* post a report to the ring */ + if (_reports->force(&new_report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -929,11 +939,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("sampling 500 samples for scaling offset"); /* set the queue depth to 10 */ - if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { - warn("failed to set queue depth"); - ret = 1; - goto out; - } + /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ +// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { +// warn("failed to set queue depth"); +// ret = 1; +// goto out; +// } /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { @@ -1188,8 +1199,7 @@ HMC5883::print_info() 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); + _reports->print_info("report queue"); } /** @@ -1293,7 +1303,12 @@ test() warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); - /* set the queue depth to 10 */ + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + + /* set the queue depth to 5 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 57c256339..bb8d45bea 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -42,7 +42,7 @@ #include <math.h> #include <stdio.h> #include <string.h> -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <unistd.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1ffca2f43..8f5674823 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -59,11 +59,12 @@ #include <nuttx/clock.h> #include <drivers/drv_hrt.h> -#include <arch/board/board.h> - #include <drivers/device/spi.h> #include <drivers/drv_gyro.h> +#include <drivers/device/ringbuffer.h> +#include <board_config.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -71,6 +72,12 @@ #endif static const int ERROR = -1; +/* Orientation on board */ +#define SENSOR_BOARD_ROTATION_000_DEG 0 +#define SENSOR_BOARD_ROTATION_090_DEG 1 +#define SENSOR_BOARD_ROTATION_180_DEG 2 +#define SENSOR_BOARD_ROTATION_270_DEG 3 + /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -78,6 +85,7 @@ static const int ERROR = -1; /* register addresses */ #define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM_H 0xD7 #define WHO_I_AM 0xD4 #define ADDR_CTRL_REG1 0x20 @@ -85,8 +93,8 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -147,6 +155,10 @@ static const int ERROR = -1; #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) +#define L3GD20_DEFAULT_RATE 760 +#define L3GD20_DEFAULT_RANGE_DPS 2000 +#define L3GD20_DEFAULT_FILTER_FREQ 30 + extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI @@ -172,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -184,10 +193,16 @@ private: orb_advert_t _gyro_topic; unsigned _current_rate; - unsigned _current_range; + unsigned _orientation; + + unsigned _read; perf_counter_t _sample_perf; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + /** * Start automatic measurement. */ @@ -199,6 +214,11 @@ private: void stop(); /** + * Reset the driver + */ + void reset(); + + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. * @@ -260,25 +280,37 @@ private: * @return OK if the value can be supported. */ int set_samplerate(unsigned frequency); -}; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + /** + * Set the lowpass filter of the driver + * + * @param samplerate The current samplerate + * @param frequency The cutoff frequency for the lowpass filter + */ + void set_driver_lowpass_filter(float samplerate, float bandwidth); + /** + * Self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); +}; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), _current_rate(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) + _orientation(SENSOR_BOARD_ROTATION_270_DEG), + _read(0), + _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), + _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) { // enable debug() calls _debug_enabled = true; @@ -299,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -315,34 +347,17 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); - /* set default configuration */ - write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ - 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 */ - - /* 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 */ + reset(); ret = OK; out: @@ -355,8 +370,29 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + bool success = false; + + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 + _orientation = SENSOR_BOARD_ROTATION_270_DEG; + #else + #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif + + success = true; + } + + + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + _orientation = SENSOR_BOARD_ROTATION_180_DEG; + success = true; + } + + if (success) return OK; return -EIO; @@ -366,6 +402,7 @@ ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); + struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -381,10 +418,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(gbuf)) { + ret += sizeof(*gbuf); + gbuf++; } } @@ -393,12 +429,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(gbuf)) { + ret = sizeof(*gbuf); + } return ret; } @@ -427,8 +464,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - return ioctl(filp, SENSORIOCSPOLLRATE, 250); + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -446,6 +482,11 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call.period = _call_interval = ticks; + /* adjust filters */ + float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f/ticks; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + /* if we need to start the poll state machine, do it */ if (want_start) start(); @@ -462,35 +503,26 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for 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 gyro_report *buf = new struct gyro_report[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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; case GYROIOCSSAMPLERATE: return set_samplerate(arg); @@ -498,10 +530,16 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGSAMPLERATE: return _current_rate; - case GYROIOCSLOWPASS: + case GYROIOCSLOWPASS: { + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + + return OK; + } + case GYROIOCGLOWPASS: - /* XXX not implemented due to wacky interaction with samplerate */ - return -EINVAL; + return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSSCALE: /* copy scale in */ @@ -514,10 +552,15 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: + /* arg should be in dps */ return set_range(arg); case GYROIOCGRANGE: - return _current_range; + /* convert to dps and round */ + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); + + case GYROIOCSELFTEST: + return self_test(); default: /* give it to the superclass */ @@ -563,28 +606,33 @@ int L3GD20::set_range(unsigned max_dps) { uint8_t bits = REG4_BDU; + float new_range_scale_dps_digit; + float new_range; - if (max_dps == 0) + if (max_dps == 0) { max_dps = 2000; - + } if (max_dps <= 250) { - _current_range = 250; + new_range = 250; bits |= RANGE_250DPS; + new_range_scale_dps_digit = 8.75e-3f; } else if (max_dps <= 500) { - _current_range = 500; + new_range = 500; bits |= RANGE_500DPS; + new_range_scale_dps_digit = 17.5e-3f; } else if (max_dps <= 2000) { - _current_range = 2000; + new_range = 2000; bits |= RANGE_2000DPS; + new_range_scale_dps_digit = 70e-3f; } else { return -EINVAL; } - _gyro_range_rad_s = _current_range / 180.0f * M_PI_F; - _gyro_range_scale = _gyro_range_rad_s / 32768.0f; + _gyro_range_rad_s = new_range / 180.0f * M_PI_F; + _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; write_reg(ADDR_CTRL_REG4, bits); return OK; @@ -598,19 +646,20 @@ L3GD20::set_samplerate(unsigned frequency) if (frequency == 0) frequency = 760; - if (frequency <= 95) { + /* use limits good for H or non-H models */ + if (frequency <= 100) { _current_rate = 95; bits |= RATE_95HZ_LP_25HZ; - } else if (frequency <= 190) { + } else if (frequency <= 200) { _current_rate = 190; bits |= RATE_190HZ_LP_25HZ; - } else if (frequency <= 380) { + } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_30HZ; + bits |= RATE_380HZ_LP_20HZ; - } else if (frequency <= 760) { + } else if (frequency <= 800) { _current_rate = 760; bits |= RATE_760HZ_LP_30HZ; @@ -624,13 +673,21 @@ L3GD20::set_samplerate(unsigned frequency) } void +L3GD20::set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth); +} + +void L3GD20::start() { /* make sure we are stopped first */ stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); @@ -643,6 +700,30 @@ L3GD20::stop() } void +L3GD20::reset() +{ + /* set default configuration */ + write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); + write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + 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 */ + + /* 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_samplerate(L3GD20_DEFAULT_RATE); + set_range(L3GD20_DEFAULT_RANGE_DPS); + set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); + + _read = 0; +} + +void L3GD20::measure_trampoline(void *arg) { L3GD20 *dev = (L3GD20 *)arg; @@ -666,7 +747,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report *report = &_reports[_next_report]; + gyro_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -689,31 +770,59 @@ L3GD20::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - report->timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = 0; // not recorded - /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - report->z_raw = raw_report.z; + switch (_orientation) { + + case SENSOR_BOARD_ROTATION_000_DEG: + /* keep axes in place */ + report.x_raw = raw_report.x; + report.y_raw = raw_report.y; + break; + + case SENSOR_BOARD_ROTATION_090_DEG: + /* swap x and y */ + report.x_raw = raw_report.y; + report.y_raw = raw_report.x; + break; + + case SENSOR_BOARD_ROTATION_180_DEG: + /* swap x and y and negate both */ + report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + break; + + case SENSOR_BOARD_ROTATION_270_DEG: + /* swap x and y and negate y */ + report.x_raw = raw_report.y; + report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + break; + } + + report.z_raw = raw_report.z; + + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; + report.x = _gyro_filter_x.apply(report.x); + report.y = _gyro_filter_y.apply(report.y); + report.z = _gyro_filter_z.apply(report.z); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + report.scaling = _gyro_range_scale; + report.range_rad_s = _gyro_range_rad_s; - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(&report); /* notify anyone waiting for data */ 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); + + _read++; /* stop the perf counter */ perf_end(_sample_perf); @@ -722,9 +831,31 @@ L3GD20::measure() void L3GD20::print_info() { + printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); +} + +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; } /** @@ -749,7 +880,7 @@ start() int fd; if (g_dev != nullptr) - errx(1, "already started"); + errx(0, "already started"); /* create the driver */ g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); @@ -838,7 +969,7 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel pollrate reset failed"); exit(0); } diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 04b565358..a37eaca53 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -52,6 +52,7 @@ __BEGIN_DECLS extern void led_init(); extern void led_on(int led); extern void led_off(int led); +extern void led_toggle(int led); __END_DECLS class LED : device::CDev @@ -98,6 +99,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg) led_off(arg); break; + case LED_TOGGLE: + led_toggle(arg); + break; + + default: result = CDev::ioctl(filp, cmd, arg); } @@ -110,11 +116,11 @@ LED *gLED; } void -drv_led_start() +drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; if (gLED != nullptr) gLED->init(); } -}
\ No newline at end of file +} diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp new file mode 100644 index 000000000..60601e22c --- /dev/null +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -0,0 +1,1638 @@ +/**************************************************************************** + * + * 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 lsm303d.cpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <stddef.h> +#include <stdlib.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 <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <nuttx/arch.h> +#include <nuttx/clock.h> + +#include <drivers/drv_hrt.h> +#include <drivers/device/spi.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_mag.h> +#include <drivers/device/ringbuffer.h> + +#include <board_config.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + + + +/* register addresses: A: accel, M: mag, T: temp */ +#define ADDR_WHO_AM_I 0x0F +#define WHO_I_AM 0x49 + +#define ADDR_OUT_L_T 0x05 +#define ADDR_OUT_H_T 0x06 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_OUT_TEMP_A 0x26 +#define ADDR_STATUS_A 0x27 +#define ADDR_OUT_X_L_A 0x28 +#define ADDR_OUT_X_H_A 0x29 +#define ADDR_OUT_Y_L_A 0x2A +#define ADDR_OUT_Y_H_A 0x2B +#define ADDR_OUT_Z_L_A 0x2C +#define ADDR_OUT_Z_H_A 0x2D + +#define ADDR_CTRL_REG0 0x1F +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 +#define ADDR_CTRL_REG6 0x25 +#define ADDR_CTRL_REG7 0x26 + +#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) +#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) +#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) + +#define REG1_BDU_UPDATE (1<<3) +#define REG1_Z_ENABLE_A (1<<2) +#define REG1_Y_ENABLE_A (1<<1) +#define REG1_X_ENABLE_A (1<<0) + +#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) +#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) +#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) + +#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) +#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) +#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) +#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) +#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) + +#define REG5_ENABLE_T (1<<7) + +#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) +#define REG5_RES_LOW_M ((0<<6) | (0<<5)) + +#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) +#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) +#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) +#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) +#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) + +#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) +#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) +#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) +#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) +#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) + +#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) + + +#define INT_CTRL_M 0x12 +#define INT_SRC_M 0x13 + +/* default values for this device */ +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 + +#define LSM303D_ONE_G 9.80665f + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + + +class LSM303D_mag; + +class LSM303D : public device::SPI +{ +public: + LSM303D(int bus, const char* path, spi_dev_e device); + virtual ~LSM303D(); + + 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(); + + friend class LSM303D_mag; + + virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + + LSM303D_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; + + struct accel_scale _accel_scale; + unsigned _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_onchip_filter_bandwith; + + struct mag_scale _mag_scale; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; + orb_advert_t _mag_topic; + + unsigned _accel_read; + unsigned _mag_read; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_sample_perf; + perf_counter_t _reg7_resets; + perf_counter_t _reg1_resets; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg7_expected; + uint8_t _reg1_expected; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + 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. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measure(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void mag_measure(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the LSM303D accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_range(unsigned max_g); + + /** + * Set the LSM303D mag measurement range. + * + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the LSM303D on-chip anti-alias filter bandwith. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); + + /** + * Set the driver lowpass filter bandwidth. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); + + /** + * Set the LSM303D internal accel sampling frequency. + * + * @param frequency The internal accel sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int accel_set_samplerate(unsigned frequency); + + /** + * Set the LSM303D internal mag sampling frequency. + * + * @param frequency The internal mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int mag_set_samplerate(unsigned frequency); +}; + +/** + * Helper class implementing the mag driver node. + */ +class LSM303D_mag : public device::CDev +{ +public: + LSM303D_mag(LSM303D *parent); + ~LSM303D_mag(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +protected: + friend class LSM303D; + + void parent_poll_notify(); +private: + LSM303D *_parent; + + void measure(); + + void measure_trampoline(void *arg); +}; + + +LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + _mag(new LSM303D_mag(this)), + _call_accel_interval(0), + _call_mag_interval(0), + _accel_reports(nullptr), + _mag_reports(nullptr), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(0), + _accel_onchip_filter_bandwith(0), + _mag_range_ga(0.0f), + _mag_range_scale(0.0f), + _mag_samplerate(0), + _accel_topic(-1), + _mag_topic(-1), + _accel_read(0), + _mag_read(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), + _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _reg1_expected(0), + _reg7_expected(0) +{ + // enable debug() calls + _debug_enabled = true; + + // default scale factors + _accel_scale.x_offset = 0.0f; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0.0f; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0.0f; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0.0f; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0.0f; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0.0f; + _mag_scale.z_scale = 1.0f; +} + +LSM303D::~LSM303D() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) + delete _accel_reports; + if (_mag_reports != nullptr) + delete _mag_reports; + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); +} + +int +LSM303D::init() +{ + int ret = ERROR; + int mag_ret; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + warnx("SPI init failed"); + goto out; + } + + /* allocate basic report buffers */ + _accel_reports = new RingBuffer(2, sizeof(accel_report)); + + if (_accel_reports == nullptr) + goto out; + + /* advertise accel topic */ + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); + + _mag_reports = new RingBuffer(2, sizeof(mag_report)); + + if (_mag_reports == nullptr) + goto out; + + reset(); + + /* advertise mag topic */ + struct mag_report zero_mag_report; + memset(&zero_mag_report, 0, sizeof(zero_mag_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); + + /* do CDev init for the mag device node, keep it optional */ + mag_ret = _mag->init(); + + if (mag_ret != OK) { + _mag_topic = -1; + } + + ret = OK; +out: + return ret; +} + +void +LSM303D::reset() +{ + /* enable accel*/ + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + write_reg(ADDR_CTRL_REG1, _reg1_expected); + + /* enable mag */ + _reg7_expected = REG7_CONT_MODE_M; + write_reg(ADDR_CTRL_REG7, _reg7_expected); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + + _accel_read = 0; + _mag_read = 0; +} + +int +LSM303D::probe() +{ + /* read dummy value to void to clear SPI statemachine on sensor */ + (void)read_reg(ADDR_WHO_AM_I); + + /* verify that the device is attached and functioning */ + bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); + + if (success) + return OK; + + return -EIO; +} + +ssize_t +LSM303D::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + accel_report *arb = reinterpret_cast<accel_report *>(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_accel_reports->get(arb)) { + ret += sizeof(*arb); + arb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + measure(); + + /* measurement will have generated a report, copy it out */ + if (_accel_reports->get(arb)) + ret = sizeof(*arb); + + return ret; +} + +ssize_t +LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + mag_report *mrb = reinterpret_cast<mag_report *>(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_mag_reports->get(mrb)) { + ret += sizeof(*mrb); + mrb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _mag_reports->flush(); + measure(); + + /* measurement will have generated a report, copy it out */ + if (_mag_reports->get(mrb)) + ret = sizeof(*mrb); + + return ret; +} + +int +LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; + + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; + + case ACCELIOCSLOWPASS: { + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; + } + } + + case ACCELIOCSRANGE: + /* arg needs to be in G */ + return accel_set_range(arg); + + case ACCELIOCGRANGE: + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSELFTEST: + return accel_self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling 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: + /* 100 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) + return -EINVAL; + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) + return SENSOR_POLLRATE_MANUAL; + + return 1000000 / _call_mag_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _mag_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case MAGIOCSSAMPLERATE: + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; + + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; + + case MAGIOCSELFTEST: + return mag_self_test(); + + case MAGIOCGEXTERNAL: + /* no external mag board yet */ + return 0; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +LSM303D::accel_self_test() +{ + if (_accel_read == 0) + 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 +LSM303D::mag_self_test() +{ + if (_mag_read == 0) + return 1; + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.y_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.z_offset) < 0.000001f) + return 1; + + return 0; +} + +uint8_t +LSM303D::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_READ; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +void +LSM303D::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +int +LSM303D::accel_set_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_scale_g_digit = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; + + } else if (max_g <= 4) { + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; + + } else if (max_g <= 6) { + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; + + } else if (max_g <= 8) { + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; + + } else if (max_g <= 16) { + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; + + } else { + return -EINVAL; + } + + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + return OK; +} + +int +LSM303D::mag_set_range(unsigned max_ga) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG6_FULL_SCALE_BITS_M; + float new_scale_ga_digit = 0.0f; + + if (max_ga == 0) + max_ga = 12; + + if (max_ga <= 2) { + _mag_range_ga = 2; + setbits |= REG6_FULL_SCALE_2GA_M; + new_scale_ga_digit = 0.080e-3f; + + } else if (max_ga <= 4) { + _mag_range_ga = 4; + setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; + + } else if (max_ga <= 8) { + _mag_range_ga = 8; + setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; + + } else if (max_ga <= 12) { + _mag_range_ga = 12; + setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; + + } else { + return -EINVAL; + } + + _mag_range_scale = new_scale_ga_digit; + + modify_reg(ADDR_CTRL_REG6, clearbits, setbits); + + return OK; +} + +int +LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_onchip_filter_bandwith = 50; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_onchip_filter_bandwith = 194; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_onchip_filter_bandwith = 362; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_onchip_filter_bandwith = 773; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); + + return OK; +} + +int +LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); + + return OK; +} + +int +LSM303D::accel_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + _accel_samplerate = 100; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + _accel_samplerate = 200; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + _accel_samplerate = 400; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + _accel_samplerate = 800; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + _accel_samplerate = 1600; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; + + return OK; +} + +int +LSM303D::mag_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG5_RATE_BITS_M; + + if (frequency == 0) + frequency = 100; + + if (frequency <= 25) { + setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; + + } else if (frequency <= 50) { + setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; + + } else if (frequency <= 100) { + setbits |= REG5_RATE_100HZ_M; + _mag_samplerate = 100; + + } else { + return -EINVAL; + } + + modify_reg(ADDR_CTRL_REG5, clearbits, setbits); + + return OK; +} + +void +LSM303D::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _accel_reports->flush(); + _mag_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this); +} + +void +LSM303D::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); +} + +void +LSM303D::measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +LSM303D::mag_measure_trampoline(void *arg) +{ + LSM303D *dev = (LSM303D *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +LSM303D::measure() +{ + if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { + perf_count(_reg1_resets); + reset(); + return; + } + + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; +#pragma pack(pop) + + accel_report accel_report; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + /* fetch data from the sensor */ + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + accel_report.timestamp = hrt_absolute_time(); + accel_report.error_count = 0; // not reported + + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = raw_accel_report.y; + accel_report.z_raw = raw_accel_report.z; + + float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); + + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; + + _accel_reports->force(&accel_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + + _accel_read++; + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +LSM303D::mag_measure() +{ + if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { + perf_count(_reg7_resets); + reset(); + return; + } + + /* status register and data as read back from the device */ +#pragma pack(push, 1) + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; +#pragma pack(pop) + + mag_report mag_report; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* fetch data from the sensor */ + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + + mag_report.timestamp = hrt_absolute_time(); + + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = raw_mag_report.y; + mag_report.z_raw = raw_mag_report.z; + mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; + + _mag_reports->force(&mag_report); + + /* XXX please check this poll_notify, is it the right one? */ + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + + _mag_read++; + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +void +LSM303D::print_info() +{ + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); + perf_print_counter(_accel_sample_perf); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); +} + +LSM303D_mag::LSM303D_mag(LSM303D *parent) : + CDev("LSM303D_mag", MAG_DEVICE_PATH), + _parent(parent) +{ +} + +LSM303D_mag::~LSM303D_mag() +{ +} + +void +LSM303D_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + return _parent->mag_ioctl(filp, cmd, arg); +} + +void +LSM303D_mag::measure() +{ + _parent->mag_measure(); +} + +void +LSM303D_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd, fd_mag; + + if (g_dev != nullptr) + errx(0, "already started"); + + /* create the driver */ + g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + + if (g_dev == nullptr) { + warnx("failed instantiating LSM303D obj"); + goto fail; + } + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + /* don't fail if open cannot be opened */ + if (0 <= fd_mag) { + if (ioctl(fd_mag, 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"); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd_accel = -1; + struct accel_report accel_report; + ssize_t sz; + int ret; + + /* get the driver */ + fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd_accel < 0) + err(1, "%s open failed", ACCEL_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd_accel, &accel_report, sizeof(accel_report)); + + if (sz != sizeof(accel_report)) + err(1, "immediate read failed"); + + + warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); + warnx("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); + warnx("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); + warnx("accel x: \t%d\traw", (int)accel_report.x_raw); + warnx("accel y: \t%d\traw", (int)accel_report.y_raw); + warnx("accel z: \t%d\traw", (int)accel_report.z_raw); + + warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + warnx("accel antialias filter bandwidth: fail"); + else + warnx("accel antialias filter bandwidth: %d Hz", ret); + + int fd_mag = -1; + struct mag_report m_report; + + /* get the driver */ + fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) + err(1, "%s open failed", MAG_DEVICE_PATH); + + /* check if mag is onboard or external */ + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("mag device active: %s", ret ? "external" : "onboard"); + + /* do a simple demand read */ + sz = read(fd_mag, &m_report, sizeof(m_report)); + + if (sz != sizeof(m_report)) + err(1, "immediate read failed"); + + warnx("mag x: \t% 9.5f\tga", (double)m_report.x); + warnx("mag y: \t% 9.5f\tga", (double)m_report.y); + warnx("mag z: \t% 9.5f\tga", (double)m_report.z); + warnx("mag x: \t%d\traw", (int)m_report.x_raw); + warnx("mag y: \t%d\traw", (int)m_report.y_raw); + warnx("mag z: \t%d\traw", (int)m_report.z_raw); + warnx("mag range: %8.4f ga", (double)m_report.range_ga); + + /* XXX add poll-rate tests here too */ + + reset(); + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(ACCEL_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, "accel pollrate reset failed"); + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + warnx("mag could not be opened, external mag might be used"); + } else { + /* no need to reset the mag as well, the reset() is the same */ + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "mag pollrate reset failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + + */ + if (!strcmp(argv[1], "start")) + lsm303d::start(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + lsm303d::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + lsm303d::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info")) + lsm303d::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk new file mode 100644 index 000000000..e40f718c5 --- /dev/null +++ b/src/drivers/lsm303d/module.mk @@ -0,0 +1,8 @@ +# +# LSM303D accel/mag driver +# + +MODULE_COMMAND = lsm303d +SRCS = lsm303d.cpp + + diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 397686e8b..c910e2890 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -59,17 +59,18 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> - #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <drivers/drv_range_finder.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> +#include <board_config.h> + /* Configuration Constants */ #define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ @@ -119,10 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -183,9 +181,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - /* * Driver 'main' command. */ @@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -221,7 +213,7 @@ MB12XX::~MB12XX() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; } int @@ -234,17 +226,15 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) debug("failed to create sensor_range_finder object. Did you start uOrb?"); @@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) 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 range_finder_report *buf = new struct range_finder_report[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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -406,6 +387,7 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; } } @@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } } while (0); @@ -498,26 +479,26 @@ MB12XX::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + struct range_finder_report report; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -525,11 +506,8 @@ MB12XX::collect() ret = OK; -out: perf_end(_sample_perf); return ret; - - return ret; } void @@ -537,7 +515,7 @@ MB12XX::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); @@ -626,8 +604,7 @@ MB12XX::print_info() 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); + _reports->print_info("report queue"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 68d2c5d65..b3003fc1b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -70,7 +70,7 @@ #include <nuttx/wqueue.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <systemlib/airspeed.h> #include <systemlib/err.h> @@ -138,12 +138,8 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } @@ -161,7 +157,8 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } @@ -169,9 +166,14 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); - + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } else if (status == 3) { log("err: fault"); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; } //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); @@ -192,27 +194,24 @@ MEASAirspeed::collect() // 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; + struct differential_pressure_s report; // 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; + if (diff_press_pa > _max_differential_pressure_pa) { + _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]); + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature; + report.differential_pressure_pa = diff_press_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* 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); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -232,7 +231,6 @@ MEASAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index e78d96569..b93f38cf6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -59,10 +59,11 @@ #include <nuttx/arch.h> #include <nuttx/i2c.h> +#include <board_config.h> + #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -#include <drivers/boards/px4fmu/px4fmu_internal.h> #include <drivers/drv_hrt.h> #include <drivers/drv_rc_input.h> @@ -74,6 +75,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/esc_status.h> #include <systemlib/err.h> @@ -94,9 +96,10 @@ class MK : public device::I2C { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; enum MappingMode { @@ -134,7 +137,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -247,7 +250,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -512,8 +515,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -539,7 +542,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; log("starting"); @@ -653,7 +656,7 @@ MK::task_main() actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ mk_servo_arm(aa.armed && !aa.lockdown); @@ -699,7 +702,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_armed); /* make sure servos are off */ @@ -1021,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + /* switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -1031,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) debug("not in a PWM mode"); break; } + */ + ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) @@ -1064,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; @@ -1345,44 +1352,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, gpio_bits = 0; servo_mode = MK::MODE_NONE; - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - /* nothing more to do here */ - break; - - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_FULL_PWM: - /* select 4-pin PWM mode */ - servo_mode = MK::MODE_4PWM; - break; - - case PORT_GPIO_AND_SERIAL: - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_SERIAL: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - /* set RX/TX multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; - break; - - case PORT_PWM_AND_GPIO: - /* select 2-pin PWM mode */ - servo_mode = MK::MODE_2PWM; - break; - } - - /* adjust GPIO config for serial mode(s) */ - if (gpio_bits != 0) - g_mk->ioctl(0, GPIO_SET_ALT_1, gpio_bits); - /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c4e331a30..70359110e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -63,7 +63,7 @@ #include <nuttx/arch.h> #include <nuttx/clock.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include <drivers/device/spi.h> @@ -149,6 +149,17 @@ #define MPU6000_REV_D9 0x59 #define MPU6000_REV_D10 0x5A +#define MPU6000_ACCEL_DEFAULT_RANGE_G 8 +#define MPU6000_ACCEL_DEFAULT_RATE 1000 +#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_GYRO_DEFAULT_RANGE_G 8 +#define MPU6000_GYRO_DEFAULT_RATE 1000 +#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 + +#define MPU6000_ONE_G 9.80665f class MPU6000_gyro; @@ -183,16 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - typedef RingBuffer<accel_report> AccelReportBuffer; - AccelReportBuffer *_accel_reports; + RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - typedef RingBuffer<gyro_report> GyroReportBuffer; - GyroReportBuffer *_gyro_reports; + RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -285,12 +294,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 */ @@ -321,6 +344,7 @@ protected: void parent_poll_notify(); private: MPU6000 *_parent; + }; /** driver 'main' command */ @@ -342,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _reads(0), _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) + _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ) { // disable debug() calls _debug_enabled = false; @@ -405,11 +429,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new AccelReportBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new GyroReportBuffer(2); + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; @@ -440,14 +464,14 @@ MPU6000::init() _gyro_topic = -1; } else { gyro_report gr; - _gyro_reports->get(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_reports->get(&ar); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: @@ -470,14 +494,13 @@ void MPU6000::reset() up_udelay(1000); // SAMPLE RATE - //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(_sample_rate); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); 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(42); + _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -520,8 +543,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (9.81f / 4096.0f); - _accel_range_m_s2 = 8.0f * 9.81f; + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; usleep(1000); @@ -633,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) accel_report *arp = reinterpret_cast<accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp++)) + if (!_accel_reports->get(arp)) break; transferred++; + arp++; } /* return the number of bytes transferred */ @@ -653,6 +677,56 @@ 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) { @@ -673,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - gyro_report *arp = reinterpret_cast<gyro_report *>(buffer); + gyro_report *grp = reinterpret_cast<gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*arp++)) + if (!_gyro_reports->get(grp)) break; transferred++; + grp++; } /* return the number of bytes transferred */ @@ -712,9 +787,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + case SENSOR_POLLRATE_DEFAULT: - /* set to same as sample rate per default */ - return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate); + return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -761,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; 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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -802,9 +869,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // 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); + _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: @@ -832,10 +899,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return _accel_range_m_s2; + return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: - return self_test(); + return accel_self_test(); default: /* give it to the superclass */ @@ -855,28 +922,19 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, cmd, arg); 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; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -915,10 +973,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_rad_s = xx return -EINVAL; case GYROIOCGRANGE: - return _gyro_range_rad_s; + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); case GYROIOCSELFTEST: - return self_test(); + return gyro_self_test(); default: /* give it to the superclass */ @@ -1128,7 +1186,7 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - + grb.error_count = arb.error_count = 0; // not reported /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1184,8 +1242,8 @@ MPU6000::measure() grb.temperature_raw = report.temp; grb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->put(arb); - _gyro_reports->put(grb); + _accel_reports->force(&arb); + _gyro_reports->force(&grb); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1204,7 +1262,10 @@ MPU6000::measure() void MPU6000::print_info() { + perf_print_counter(_sample_perf); printf("reads: %u\n", _reads); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : @@ -1335,7 +1396,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / 9.81f)); + (double)(a_report.range_m_s2 / MPU6000_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk index 3c4b0f093..20f8aa173 100644 --- a/src/drivers/ms5611/module.mk +++ b/src/drivers/ms5611/module.mk @@ -37,4 +37,4 @@ MODULE_COMMAND = ms5611 -SRCS = ms5611.cpp +SRCS = ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index c13b65d5a..938786d3f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.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 @@ -33,13 +33,11 @@ /** * @file ms5611.cpp - * Driver for the MS5611 barometric pressure sensor connected via I2C. + * Driver for the MS5611 barometric pressure sensor connected via I2C or SPI. */ #include <nuttx/config.h> -#include <drivers/device/i2c.h> - #include <sys/types.h> #include <stdint.h> #include <stdbool.h> @@ -59,12 +57,15 @@ #include <arch/board/board.h> +#include <drivers/device/device.h> +#include <drivers/drv_baro.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> -#include <drivers/drv_baro.h> +#include "ms5611.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -76,35 +77,25 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -/** - * Calibration PROM as reported by the device. - */ -#pragma pack(push,1) -struct ms5611_prom_s { - uint16_t factory_setup; - uint16_t c1_pressure_sens; - uint16_t c2_pressure_offset; - uint16_t c3_temp_coeff_pres_sens; - uint16_t c4_temp_coeff_pres_offset; - uint16_t c5_reference_temp; - uint16_t c6_temp_coeff_temp; - uint16_t serial_and_crc; -}; +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) -/** - * Grody hack for crc4() +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +/* + * MS5611 internal constants and data structures. */ -union ms5611_prom_u { - uint16_t c[8]; - struct ms5611_prom_s s; -}; -#pragma pack(pop) -class MS5611 : public device::I2C +/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ +#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +class MS5611 : public device::CDev { public: - MS5611(int bus); - virtual ~MS5611(); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf); + ~MS5611(); virtual int init(); @@ -117,18 +108,14 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; -private: - union ms5611_prom_u _prom; + ms5611::prom_s _prom; struct work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -149,16 +136,7 @@ private: 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. + * Initialize 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. @@ -198,74 +176,24 @@ private: * * @return OK if the measurement command was successful. */ - int measure(); + virtual int measure(); /** * Collect the result of the most recent measurement. */ - int collect(); - - /** - * Send a reset command to the MS5611. - * - * This is required after any bus reset. - */ - int cmd_reset(); - - /** - * Read the MS5611 PROM - * - * @return OK if the PROM reads successfully. - */ - int read_prom(); - - /** - * PROM CRC routine ported from MS5611 application note - * - * @param n_prom Pointer to words read from PROM. - * @return True if the CRC matches. - */ - bool crc4(uint16_t *n_prom); - + virtual int collect(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -/* - * MS5611 internal constants and data structures. - */ - -/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ -#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ - -#define MS5611_BUS PX4_I2C_BUS_ONBOARD -#define MS5611_ADDRESS_1 PX4_I2C_OBDEV_MS5611 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ - -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ - /* * Driver 'main' command. */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); - -MS5611::MS5611(int bus) : - I2C("MS5611", BARO_DEVICE_PATH, bus, 0, 400000), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : + CDev("MS5611", BARO_DEVICE_PATH), + _interface(interface), + _prom(prom_buf.s), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -279,10 +207,7 @@ MS5611::MS5611(int bus) : _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { - // enable debug() calls - _debug_enabled = true; - - // work_cancel in the dtor will explode if we don't do this... + // work_cancel in stop_cycle called from the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -293,78 +218,58 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + + delete _interface; } int MS5611::init() { - int ret = ERROR; + int ret; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(baro_report)); - if (_reports == nullptr) + if (_reports == nullptr) { + debug("can't get memory for reports"); + ret = -ENOMEM; goto out; - - _oldest_report = _next_report = 0; + } /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + struct baro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); - if (_baro_topic < 0) + if (_baro_topic < 0) { debug("failed to create sensor_baro object"); + ret = -ENOSPC; + goto out; + } ret = OK; out: return ret; } -int -MS5611::probe() -{ - _retries = 10; - - if ((OK == probe_address(MS5611_ADDRESS_1)) || - (OK == probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; - return OK; - } - - return -EIO; -} - -int -MS5611::probe_address(uint8_t address) -{ - /* select the address we are going to try */ - set_address(address); - - /* send reset command */ - if (OK != cmd_reset()) - return -EIO; - - /* read PROM */ - if (OK != read_prom()) - return -EIO; - - return OK; -} - ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -380,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * 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 (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; } } @@ -392,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* do temperature first */ if (OK != measure()) { @@ -424,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(brp)) + ret = sizeof(*brp); } while (0); @@ -500,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) 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 baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - return OK; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -546,8 +439,9 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) break; } - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + /* give it to the bus-specific superclass */ + // return bus_ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } void @@ -557,7 +451,7 @@ MS5611::start_cycle() /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); @@ -572,7 +466,7 @@ MS5611::stop_cycle() void MS5611::cycle_trampoline(void *arg) { - MS5611 *dev = (MS5611 *)arg; + MS5611 *dev = reinterpret_cast<MS5611 *>(arg); dev->cycle(); } @@ -592,7 +486,7 @@ MS5611::cycle() /* * The ms5611 seems to regularly fail to respond to * its address; this happens often enough that we'd rather not - * spam the console with the message. + * spam the console with a message for this. */ } else { //log("collection error %d", ret); @@ -654,17 +548,12 @@ MS5611::measure() /* * In phase zero, request temperature; in other phases, request pressure. */ - uint8_t cmd_data = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; /* * Send the command to begin measuring. - * - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the write. */ - _retries = 0; - ret = transfer(&cmd_data, 1, nullptr, 0); - + ret = _interface->ioctl(IOCTL_MEASURE, addr); if (OK != ret) perf_count(_comms_errors); @@ -677,47 +566,35 @@ int MS5611::collect() { int ret; - uint8_t cmd; - uint8_t data[3]; - union { - uint8_t b[4]; - uint32_t w; - } cvt; - - /* read the most recent measurement */ - cmd = 0; + uint32_t raw; perf_begin(_sample_perf); + struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); - ret = transfer(&cmd, 1, &data[0], 3); - if (ret != OK) { + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + ret = _interface->read(0, (void *)&raw, 0); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - /* fetch the raw value */ - cvt.b[0] = data[2]; - cvt.b[1] = data[1]; - cvt.b[2] = data[0]; - cvt.b[3] = 0; - uint32_t raw = cvt.w; - /* handle a measurement */ if (_measure_phase == 0) { /* temperature offset (in ADC units) */ - int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + int32_t dT = (int32_t)raw - ((int32_t)_prom.c5_reference_temp << 8); /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ - _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + _TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.c6_temp_coeff_temp) >> 23); /* base sensor scale/offset values */ - _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); - _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.c1_pressure_sens << 15) + (((int64_t)_prom.c3_temp_coeff_pres_sens * dT) >> 8); + _OFF = ((int64_t)_prom.c2_pressure_offset << 16) + (((int64_t)_prom.c4_temp_coeff_pres_offset * dT) >> 7); /* temperature compensation */ if (_TEMP < 2000) { @@ -745,8 +622,8 @@ MS5611::collect() int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ @@ -761,30 +638,7 @@ MS5611::collect() * double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us * single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us */ -#if 0/* USE_FLOAT */ - /* tropospheric properties (0-11km) for standard atmosphere */ - const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */ - const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */ - const float g = 9.80665f; /* gravity constant in m/s/s */ - const float R = 287.05f; /* ideal gas constant in J/kg/K */ - - /* current pressure at MSL in kPa */ - float p1 = _msl_pressure / 1000.0f; - - /* measured pressure in kPa */ - float p = P / 1000.0f; - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#else /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ @@ -806,18 +660,13 @@ MS5611::collect() * h = ------------------------------- + h1 * a */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; -#endif - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* publish it */ + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -832,55 +681,48 @@ MS5611::collect() return OK; } -int -MS5611::cmd_reset() +void +MS5611::print_info() { - unsigned old_retrycount = _retries; - uint8_t cmd = ADDR_RESET_CMD; - int result; - - /* bump the retry count */ - _retries = 10; - result = transfer(&cmd, 1, nullptr, 0); - _retries = old_retrycount; + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); + printf("TEMP: %d\n", _TEMP); + printf("SENS: %lld\n", _SENS); + printf("OFF: %lld\n", _OFF); + printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - return result; + printf("factory_setup %u\n", _prom.factory_setup); + printf("c1_pressure_sens %u\n", _prom.c1_pressure_sens); + printf("c2_pressure_offset %u\n", _prom.c2_pressure_offset); + printf("c3_temp_coeff_pres_sens %u\n", _prom.c3_temp_coeff_pres_sens); + printf("c4_temp_coeff_pres_offset %u\n", _prom.c4_temp_coeff_pres_offset); + printf("c5_reference_temp %u\n", _prom.c5_reference_temp); + printf("c6_temp_coeff_temp %u\n", _prom.c6_temp_coeff_temp); + printf("serial_and_crc %u\n", _prom.serial_and_crc); } -int -MS5611::read_prom() +/** + * Local functions in support of the shell command. + */ +namespace ms5611 { - uint8_t prom_buf[2]; - union { - uint8_t b[2]; - uint16_t w; - } cvt; - - /* - * Wait for PROM contents to be in the device (2.8 ms) in the case we are - * called immediately after reset. - */ - usleep(3000); - - /* read and convert PROM words */ - for (int i = 0; i < 8; i++) { - uint8_t cmd = ADDR_PROM_SETUP + (i * 2); - if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) - break; - - /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ - cvt.b[0] = prom_buf[1]; - cvt.b[1] = prom_buf[0]; - _prom.c[i] = cvt.w; - } +MS5611 *g_dev; - /* calculate CRC and return success/failure accordingly */ - return crc4(&_prom.c[0]) ? OK : -EIO; -} +void start(); +void test(); +void reset(); +void info(); +void calibrate(unsigned altitude); +/** + * MS5611 crc4 cribbed from the datasheet + */ bool -MS5611::crc4(uint16_t *n_prom) +crc4(uint16_t *n_prom) { int16_t cnt; uint16_t n_rem; @@ -922,43 +764,6 @@ MS5611::crc4(uint16_t *n_prom) return (0x000F & crc_read) == (n_rem ^ 0x00); } -void -MS5611::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); - printf("TEMP: %d\n", _TEMP); - printf("SENS: %lld\n", _SENS); - printf("OFF: %lld\n", _OFF); - printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); - - printf("factory_setup %u\n", _prom.s.factory_setup); - printf("c1_pressure_sens %u\n", _prom.s.c1_pressure_sens); - printf("c2_pressure_offset %u\n", _prom.s.c2_pressure_offset); - printf("c3_temp_coeff_pres_sens %u\n", _prom.s.c3_temp_coeff_pres_sens); - printf("c4_temp_coeff_pres_offset %u\n", _prom.s.c4_temp_coeff_pres_offset); - printf("c5_reference_temp %u\n", _prom.s.c5_reference_temp); - printf("c6_temp_coeff_temp %u\n", _prom.s.c6_temp_coeff_temp); - printf("serial_and_crc %u\n", _prom.s.serial_and_crc); -} - -/** - * Local functions in support of the shell command. - */ -namespace ms5611 -{ - -MS5611 *g_dev; - -void start(); -void test(); -void reset(); -void info(); -void calibrate(unsigned altitude); /** * Start the driver. @@ -967,28 +772,46 @@ void start() { int fd; + prom_u prom_buf; if (g_dev != nullptr) /* if already started, the still command succeeded */ errx(0, "already started"); - /* create the driver */ - g_dev = new MS5611(MS5611_BUS); + device::Device *interface = nullptr; - if (g_dev == nullptr) - goto fail; + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ + if (MS5611_spi_interface != nullptr) + interface = MS5611_spi_interface(prom_buf); + if (interface == nullptr && (MS5611_i2c_interface != nullptr)) + interface = MS5611_i2c_interface(prom_buf); + + if (interface == nullptr) + errx(1, "failed to allocate an interface"); + + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } - if (OK != g_dev->init()) + g_dev = new MS5611(interface, prom_buf); + if (g_dev == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev->init() != OK) goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(BARO_DEVICE_PATH, O_RDONLY); - - if (fd < 0) + if (fd < 0) { + warnx("can't open baro device"); goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + } + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + warnx("failed setting default poll rate"); goto fail; + } exit(0); @@ -1155,11 +978,11 @@ calibrate(unsigned altitude) const float g = 9.80665f; /* gravity constant in m/s/s */ const float R = 287.05f; /* ideal gas constant in J/kg/K */ - warnx("averaged pressure %10.4fkPa at %um", pressure, altitude); + warnx("averaged pressure %10.4fkPa at %um", (double)pressure, altitude); p1 = pressure * (powf(((T1 + (a * (float)altitude)) / T1), (g / (a * R)))); - warnx("calculated MSL pressure %10.4fkPa", p1); + warnx("calculated MSL pressure %10.4fkPa", (double)p1); /* save as integer Pa */ p1 *= 1000.0f; diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h new file mode 100644 index 000000000..76fb84de8 --- /dev/null +++ b/src/drivers/ms5611/ms5611.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file ms5611.h + * + * Shared defines for the ms5611 driver. + */ + +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +namespace ms5611 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t factory_setup; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t serial_and_crc; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +extern bool crc4(uint16_t *n_prom); + +} /* namespace */ + +/* interface factories */ +extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function; +extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function; + diff --git a/src/drivers/ms5611/ms5611_i2c.cpp b/src/drivers/ms5611/ms5611_i2c.cpp new file mode 100644 index 000000000..87d9b94a6 --- /dev/null +++ b/src/drivers/ms5611/ms5611_i2c.cpp @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * 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 ms5611_i2c.cpp + * + * I2C interface for MS5611 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/i2c.h> + +#include "ms5611.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_MS5611 + +#ifndef PX4_I2C_BUS_ONBOARD + #define MS5611_BUS 1 +#else + #define MS5611_BUS PX4_I2C_BUS_ONBOARD +#endif + +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + + + +device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf); + +class MS5611_I2C : public device::I2C +{ +public: + MS5611_I2C(int bus, ms5611::prom_u &prom_buf); + virtual ~MS5611_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +private: + ms5611::prom_u &_prom; + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + +}; + +device::Device * +MS5611_i2c_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_I2C(MS5611_BUS, prom_buf); +} + +MS5611_I2C::MS5611_I2C(int bus, ms5611::prom_u &prom) : + I2C("MS5611_I2C", nullptr, bus, 0, 400000), + _prom(prom) +{ +} + +MS5611_I2C::~MS5611_I2C() +{ +} + +int +MS5611_I2C::init() +{ + /* this will call probe(), and thereby _probe_address */ + return I2C::init(); +} + +int +MS5611_I2C::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +int +MS5611_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + return ret; +} + +int +MS5611_I2C::probe() +{ + _retries = 10; + + if ((OK == _probe_address(MS5611_ADDRESS_1)) || + (OK == _probe_address(MS5611_ADDRESS_2))) { + /* + * Disable retries; we may enable them selectively in some cases, + * but the device gets confused if we retry some of the commands. + */ + _retries = 0; + return OK; + } + + return -EIO; +} + +int +MS5611_I2C::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_address(address); + + /* send reset command */ + if (OK != _reset()) + return -EIO; + + /* read PROM */ + if (OK != _read_prom()) + return -EIO; + + return OK; +} + + +int +MS5611_I2C::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = ADDR_RESET_CMD; + int result; + + /* bump the retry count */ + _retries = 10; + result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int +MS5611_I2C::_measure(unsigned addr) +{ + /* + * Disable retries on this command; we can't know whether failure + * means the device did or did not see the command. + */ + _retries = 0; + + uint8_t cmd = addr; + return transfer(&cmd, 1, nullptr, 0); +} + +int +MS5611_I2C::_read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = ADDR_PROM_SETUP + (i * 2); + + if (OK != transfer(&cmd, 1, &prom_buf[0], 2)) + break; + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +#endif /* PX4_I2C_OBDEV_MS5611 */ diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp new file mode 100644 index 000000000..e547c913b --- /dev/null +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * 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 ms5611_spi.cpp + * + * SPI interface for MS5611 + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> + +#include <drivers/device/spi.h> + +#include "ms5611.h" +#include "board_config.h" + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#ifdef PX4_SPIDEV_BARO + +device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf); + +class MS5611_SPI : public device::SPI +{ +public: + MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf); + virtual ~MS5611_SPI(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + ms5611::prom_u &_prom; + + /** + * Send a reset command to the MS5611. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Send a measure command to the MS5611. + * + * @param addr Which address to use for the measure operation. + */ + int _measure(unsigned addr); + + /** + * Read the MS5611 PROM + * + * @return OK if the PROM reads successfully. + */ + int _read_prom(); + + /** + * Read a 16-bit register value. + * + * @param reg The register to read. + */ + uint16_t _reg16(unsigned reg); + + /** + * Wrapper around transfer() that prevents interrupt-context transfers + * from pre-empting us. The sensor may (does) share a bus with sensors + * that are polled from interrupt context (or we may be pre-empted) + * so we need to guarantee that transfers complete without interruption. + */ + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); +}; + +device::Device * +MS5611_spi_interface(ms5611::prom_u &prom_buf) +{ + return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); +} + +MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + _prom(prom_buf) +{ +} + +MS5611_SPI::~MS5611_SPI() +{ +} + +int +MS5611_SPI::init() +{ + int ret; + irqstate_t flags; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + goto out; + } + + /* send reset command */ + ret = _reset(); + if (ret != OK) { + debug("reset failed"); + goto out; + } + + /* read PROM */ + ret = _read_prom(); + if (ret != OK) { + debug("prom readout failed"); + goto out; + } + +out: + return ret; +} + +int +MS5611_SPI::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[4]; + + /* read the most recent measurement */ + buf[0] = 0 | DIR_WRITE; + int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); + + if (ret == OK) { + /* fetch the raw value */ + cvt->b[0] = buf[3]; + cvt->b[1] = buf[2]; + cvt->b[2] = buf[1]; + cvt->b[3] = 0; + + ret = count; + } + + return ret; +} + +int +MS5611_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + case IOCTL_RESET: + ret = _reset(); + break; + + case IOCTL_MEASURE: + ret = _measure(arg); + break; + + default: + ret = EINVAL; + } + + if (ret != OK) { + errno = ret; + return -1; + } + return 0; +} + +int +MS5611_SPI::_reset() +{ + uint8_t cmd = ADDR_RESET_CMD | DIR_WRITE; + + return _transfer(&cmd, nullptr, 1); +} + +int +MS5611_SPI::_measure(unsigned addr) +{ + uint8_t cmd = addr | DIR_WRITE; + + return _transfer(&cmd, nullptr, 1); +} + + +int +MS5611_SPI::_read_prom() +{ + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + usleep(3000); + + /* read and convert PROM words */ + for (int i = 0; i < 8; i++) { + uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); + _prom.c[i] = _reg16(cmd); + } + + /* calculate CRC and return success/failure accordingly */ + return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; +} + +uint16_t +MS5611_SPI::_reg16(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = reg | DIR_READ; + + _transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +int +MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + return transfer(send, recv, len); +} + +#endif /* PX4_SPIDEV_BARO */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a98b527b1..0441566e9 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.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 @@ -34,7 +34,7 @@ /** * @file fmu.cpp * - * Driver/configurator for the PX4 FMU multi-purpose port. + * Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards. */ #include <nuttx/config.h> @@ -57,29 +57,41 @@ #include <drivers/device/device.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> -#include <drivers/boards/px4fmu/px4fmu_internal.h> #include <drivers/drv_hrt.h> +#include <board_config.h> + #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <drivers/drv_mixer.h> #include <drivers/drv_rc_input.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_armed.h> -#include <systemlib/err.h> -#include <systemlib/ppm_decode.h> + +#ifdef HRT_PPM_CHANNEL +# include <systemlib/ppm_decode.h> +#endif + +/* + * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side + */ + +#define CONTROL_INPUT_DROP_LIMIT_MS 20 class PX4FMU : public device::CDev { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -95,7 +107,12 @@ public: int set_pwm_alt_channels(uint32_t channels); private: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + static const unsigned _max_actuators = 6; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -104,7 +121,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -112,11 +129,20 @@ private: volatile bool _task_should_exit; bool _armed; + bool _pwm_on; MixerGroup *_mixers; actuator_controls_s _controls; + pwm_limit_t _pwm_limit; + uint16_t _failsafe_pwm[_max_actuators]; + uint16_t _disarmed_pwm[_max_actuators]; + uint16_t _min_pwm[_max_actuators]; + uint16_t _max_pwm[_max_actuators]; + unsigned _num_failsafe_set; + unsigned _num_disarmed_set; + static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -146,6 +172,7 @@ private: }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1}, @@ -154,6 +181,22 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1}, {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2}, {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2}, +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, + {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {0, GPIO_VDD_3V3_SENSORS_EN, 0}, + {GPIO_VDD_BRICK_VALID, 0, 0}, + {GPIO_VDD_SERVO_VALID, 0, 0}, + {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, + {GPIO_VDD_5V_PERIPH_OC, 0, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -174,15 +217,25 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), _armed(false), - _mixers(nullptr) + _pwm_on(false), + _mixers(nullptr), + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { + for (unsigned i = 0; i < _max_actuators; i++) { + _min_pwm[i] = PWM_DEFAULT_MIN; + _max_pwm[i] = PWM_DEFAULT_MAX; + } + _debug_enabled = true; } @@ -271,9 +324,36 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: // multi-port with flow control lines as PWM - case MODE_4PWM: // multi-port as 4 PWM outs - debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + debug("MODE_2PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0x3); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_4PWM: // v1 multi-port as 4 PWM outs + debug("MODE_4PWM"); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + + break; + + case MODE_6PWM: // v2 PWMs as 6 PWM outs + debug("MODE_6PWM"); /* default output rates */ _pwm_default_rate = 50; @@ -281,7 +361,7 @@ PX4FMU::set_mode(Mode mode) _pwm_alt_rate_channels = 0; /* XXX magic numbers */ - up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + up_pwm_servo_init(0x3f); set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); break; @@ -376,8 +456,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,17 +476,20 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; - unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; - +#ifdef HRT_PPM_CHANNEL // rc input, published to ORB struct rc_input_values rc_in; orb_advert_t to_input_rc = 0; memset(&rc_in, 0, sizeof(rc_in)); rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; +#endif + + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); log("starting"); @@ -442,76 +525,107 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); + int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); usleep(1000000); continue; - } + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; + } else { + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; } - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); - } + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + /* output actual limited values */ + for (unsigned i = 0; i < num_outputs; i++) { + controls_effective.control_effective[i] = (float)pwm_limited[i]; + } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } } - } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + /* update the armed status and check that we're not locked down */ + bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) + _armed = set_armed; - /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; - if (set_armed != _armed) { - _armed = set_armed; - up_pwm_servo_arm(set_armed); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } } } +#ifdef HRT_PPM_CHANNEL // see if we have new PPM input data if (ppm_last_valid_decode != rc_in.timestamp) { // we have a new PPM frame. Publish it. @@ -531,11 +645,13 @@ PX4FMU::task_main() orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); } } +#endif + } ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -579,6 +695,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -615,25 +732,183 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(false); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_default_rate; + break; + case PWM_SERVO_SET_UPDATE_RATE: ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate; + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; - case PWM_SERVO_SET(2): + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate_channels; + break; + + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _failsafe_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _failsafe_pwm[i] = PWM_LOWEST_MIN; + } else { + _failsafe_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_failsafe_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_failsafe_pwm[i] > 0) + _num_failsafe_set++; + } + break; + } + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _disarmed_pwm[i] = PWM_HIGHEST_MAX; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _disarmed_pwm[i] = PWM_LOWEST_MIN; + } else { + _disarmed_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } + break; + } + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MIN) { + _min_pwm[i] = PWM_HIGHEST_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MIN) { + _min_pwm[i] = PWM_LOWEST_MIN; + } else { + _min_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _max_pwm[i] = PWM_HIGHEST_MAX; + } else { + _max_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _max_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + + case PWM_SERVO_SET(5): + case PWM_SERVO_SET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_SET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_SET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): - if (arg < 2100) { + case PWM_SERVO_SET(0): + if (arg <= 2100) { up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; @@ -641,16 +916,24 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_mode < MODE_6PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(2): + if (_mode < MODE_4PWM) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); break; @@ -658,16 +941,26 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(1): case PWM_SERVO_GET_RATEGROUP(2): case PWM_SERVO_GET_RATEGROUP(3): + case PWM_SERVO_GET_RATEGROUP(4): + case PWM_SERVO_GET_RATEGROUP(5): *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { + switch (_mode) { + case MODE_6PWM: + *(unsigned *)arg = 6; + break; + case MODE_4PWM: *(unsigned *)arg = 4; - - } else { + break; + case MODE_2PWM: *(unsigned *)arg = 2; + break; + default: + ret = -EINVAL; + break; } break; @@ -743,17 +1036,17 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[4]; + uint16_t values[6]; - if (count > 4) { - // we only have 4 PWM outputs on the FMU - count = 4; + if (count > 6) { + // we have at most 6 outputs + count = 6; } // allow for misaligned values - memcpy(values, buffer, count*2); + memcpy(values, buffer, count * 2); - for (uint8_t i=0; i<count; i++) { + for (uint8_t i = 0; i < count; i++) { up_pwm_servo_set(i, values[i]); } return count * 2; @@ -763,19 +1056,28 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs, GPIO driver chip - * to input mode. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + /* if we have a GPIO direction control, set it to zero (input) */ stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_configgpio(GPIO_GPIO_DIR); +#endif } void PX4FMU::gpio_set_function(uint32_t gpios, int function) { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* * GPIOs 0 and 1 must have the same direction as they are buffered * by a shared 2-port driver. Any attempt to set either sets both. @@ -787,6 +1089,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) if (GPIO_SET_OUTPUT == function) stm32_gpiowrite(GPIO_GPIO_DIR, 1); } +#endif /* configure selected GPIOs as required */ for (unsigned i = 0; i < _ngpio; i++) { @@ -809,9 +1112,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function) } } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* flip buffer to input mode if required */ if ((GPIO_SET_INPUT == function) && (gpios & 3)) stm32_gpiowrite(GPIO_GPIO_DIR, 0); +#endif } void @@ -912,14 +1217,21 @@ fmu_new_mode(PortMode new_mode) /* nothing more to do here */ break; - case PORT_FULL_SERIAL: - /* set all multi-GPIOs to serial mode */ - gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; - break; - case PORT_FULL_PWM: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) /* select 4-pin PWM mode */ servo_mode = PX4FMU::MODE_4PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + servo_mode = PX4FMU::MODE_6PWM; +#endif + break; + + /* mixed modes supported on v1 board only */ +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + case PORT_FULL_SERIAL: + /* set all multi-GPIOs to serial mode */ + gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; break; case PORT_GPIO_AND_SERIAL: @@ -938,6 +1250,9 @@ fmu_new_mode(PortMode new_mode) /* select 2-pin PWM mode */ servo_mode = PX4FMU::MODE_2PWM; break; +#endif + default: + return -1; } /* adjust GPIO config for serial mode(s) */ @@ -975,25 +1290,104 @@ fmu_start(void) return ret; } +int +fmu_stop(void) +{ + int ret = OK; + + if (g_fmu != nullptr) { + + delete g_fmu; + g_fmu = nullptr; + } + + return ret; +} + void test(void) { - int fd; - - fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + int fd; + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + int ret; + + fd = open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) errx(1, "open fail"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); - if (ioctl(fd, PWM_SERVO_SET(0), 1000) < 0) err(1, "servo 1 set failed"); + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { + err(1, "Unable to get servo count\n"); + } + + warnx("Testing %u servos", (unsigned)servo_count); + + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Press CTRL-C or 'c' to abort."); + + for (;;) { + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + if (direction == 1) { + // use ioctl interface for one direction + for (unsigned i=0; i < servo_count; i++) { + if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { + err(1, "servo %u set failed", i); + } + } + } else { + // and use write interface for the other direction + ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + } + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } - if (ioctl(fd, PWM_SERVO_SET(1), 1200) < 0) err(1, "servo 2 set failed"); + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; - if (ioctl(fd, PWM_SERVO_SET(2), 1400) < 0) err(1, "servo 3 set failed"); + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } - if (ioctl(fd, PWM_SERVO_SET(3), 1600) < 0) err(1, "servo 4 set failed"); + /* Check if user wants to quit */ + char c; + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + break; + } + } + } close(fd); @@ -1044,6 +1438,12 @@ fmu_main(int argc, char *argv[]) PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[1]; + if (!strcmp(verb, "stop")) { + fmu_stop(); + errx(0, "FMU driver stopped"); + } + + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); @@ -1053,12 +1453,13 @@ fmu_main(int argc, char *argv[]) if (!strcmp(verb, "mode_gpio")) { new_mode = PORT_FULL_GPIO; - } else if (!strcmp(verb, "mode_serial")) { - new_mode = PORT_FULL_SERIAL; - } else if (!strcmp(verb, "mode_pwm")) { new_mode = PORT_FULL_PWM; +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(verb, "mode_gpio_serial")) { new_mode = PORT_GPIO_AND_SERIAL; @@ -1067,6 +1468,7 @@ fmu_main(int argc, char *argv[]) } else if (!strcmp(verb, "mode_pwm_gpio")) { new_mode = PORT_PWM_AND_GPIO; +#endif } /* was a new mode set? */ @@ -1088,6 +1490,10 @@ fmu_main(int argc, char *argv[]) fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + fprintf(stderr, " mode_gpio, mode_pwm, test\n"); +#endif exit(1); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b3..d918abd57 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp +SRCS = fmu.cpp \ + ../../modules/systemlib/pwm_limit/pwm_limit.c diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 328e5a684..2054faa12 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -38,4 +38,9 @@ MODULE_COMMAND = px4io SRCS = px4io.cpp \ - uploader.cpp + px4io_uploader.cpp \ + px4io_serial.cpp \ + px4io_i2c.cpp + +# XXX prune to just get UART registers +INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index fed83ea1a..08e697b9f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -58,7 +58,6 @@ #include <arch/board/board.h> #include <drivers/device/device.h> -#include <drivers/device/i2c.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_gpio.h> @@ -75,51 +74,71 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/safety.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/servorail_status.h> #include <uORB/topics/parameter_update.h> + #include <debug.h> #include <mavlink/mavlink_log.h> -#include "uploader.h" #include <modules/px4iofirmware/protocol.h> +#include "uploader.h" + +extern device::Device *PX4IO_i2c_interface() weak_function; +extern device::Device *PX4IO_serial_interface() weak_function; + #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz +#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz +#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz + /** * The PX4IO class. * * Encapsulates PX4FMU to PX4IO communications modeled as file operations. */ -class PX4IO : public device::I2C +class PX4IO : public device::CDev { public: /** * Constructor. - * + * * Initialize all class variables. */ - PX4IO(); + PX4IO(device::Device *interface); + /** * 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. + * + * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); /** + * Detect if a PX4IO is connected. + * + * Only validate if there is a PX4IO to talk to. + */ + virtual int detect(); + + /** * 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 @@ -130,7 +149,7 @@ public: /** * 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 @@ -143,8 +162,8 @@ public: /** * Set the update rate for actuator outputs from FMU to IO. * - * @param[in] rate The rate in Hz actuator output are sent to IO. - * Min 10 Hz, max 400 Hz + * @param[in] rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -159,12 +178,17 @@ public: /** * Push failsafe values to IO. * - * @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 + * @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); /** + * Disable RC input handling + */ + int disable_rc_handling(); + + /** * Print IO status. * * Print all relevant IO status information @@ -172,12 +196,17 @@ public: void print_status(); /** + * Fetch and print debug console output. + */ + int print_debug(); + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /** * 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) - { + inline void set_dsm_vcc_ctl(bool enable) { _dsm_vcc_ctl = enable; }; @@ -186,25 +215,32 @@ public: * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() - { + inline bool get_dsm_vcc_ctl() { return _dsm_vcc_ctl; }; +#endif + + inline uint16_t system_status() const {return _status;} private: + device::Device *_interface; + // XXX + unsigned _hardware; ///< Hardware revision 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 + bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values volatile int _task; ///<worker task id volatile bool _task_should_exit; ///<worker terminate flag - int _mavlink_fd; ///<mavlink file descriptor + int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. + int _thread_mavlink_fd; ///<mavlink file descriptor for thread. perf_counter_t _perf_update; ///<local performance counter @@ -213,16 +249,19 @@ private: 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_actuator_armed; ///< system armed control topic + int _t_vehicle_control_mode;///< vehicle control mode topic + int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ - 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 + 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_servorail; ///< servorail status + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///<mixed outputs actuator_controls_effective_s _controls_effective; ///<effective controls @@ -234,7 +273,9 @@ private: float _battery_mamphour_total;///<amp hours consumed so far uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power +#endif /** * Trampoline to the worker task @@ -269,6 +310,11 @@ private: int io_get_status(); /** + * Disable RC input handling + */ + int io_disable_rc_handling(); + + /** * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. @@ -298,7 +344,7 @@ private: * @param offset Register offset to start writing at. * @param values Pointer to array of values to write. * @param num_values The number of values to write. - * @return Zero if all values were successfully written. + * @return OK if all values were successfully written. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); @@ -308,7 +354,7 @@ private: * @param page Register page to write to. * @param offset Register offset to write to. * @param value Value to write. - * @return Zero if the value was written successfully. + * @return OK if the value was written successfully. */ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); @@ -319,7 +365,7 @@ private: * @param offset Register offset to start reading from. * @param values Pointer to array where values should be stored. * @param num_values The number of values to read. - * @return Zero if all values were successfully read. + * @return OK if all values were successfully read. */ int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); @@ -346,7 +392,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen); + int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3); /** * Handle a status update from IO. @@ -366,8 +412,34 @@ private: */ int io_handle_alarms(uint16_t alarms); -}; + /** + * Handle issuing dsm bind ioctl to px4io. + * + * @param dsmMode 0:dsm2, 1:dsmx + */ + void dsm_bind_ioctl(int dsmMode); + /** + * Handle a battery update from IO. + * + * Publish IO battery information if necessary. + * + * @param vbatt vbatt register + * @param ibatt ibatt register + */ + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); + + /** + * Handle a servorail update from IO. + * + * Publish servo rail information if necessary. + * + * @param vservo vservo register + * @param vrssi vrssi register + */ + void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + +}; namespace { @@ -376,34 +448,44 @@ PX4IO *g_dev; } -PX4IO::PX4IO() : - I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), +PX4IO::PX4IO(device::Device *interface) : + CDev("px4io", PX4IO_DEVICE_PATH), + _interface(interface), + _hardware(0), _max_actuators(0), _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), + _rc_handling_disabled(false), _task(-1), _task_should_exit(false), _mavlink_fd(-1), + _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), - _t_vstatus(-1), + _t_actuator_armed(-1), + _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_servorail(0), + _to_safety(0), _primary_pwm_device(false), - _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor + _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), - _dsm_vcc_ctl(false) + _battery_last_timestamp(0) +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + , _dsm_vcc_ctl(false) +#endif + { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -429,10 +511,47 @@ PX4IO::~PX4IO() if (_task != -1) task_delete(_task); + if (_interface != nullptr) + delete _interface; + g_dev = nullptr; } int +PX4IO::detect() +{ + int ret; + + if (_task == -1) { + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + if (protocol == _io_reg_get_error) { + log("IO not installed"); + + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + + return -1; + } + } + + log("IO found"); + + return 0; +} + +int PX4IO::init() { int ret; @@ -440,33 +559,37 @@ PX4IO::init() ASSERT(_task == -1); /* do regular cdev init */ - ret = I2C::init(); + ret = CDev::init(); + if (ret != OK) return ret; - /* - * Enable a couple of retries for operations to IO. - * - * Register read/write operations are intentionally idempotent - * so this is safe as designed. - */ - _retries = 2; - /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + log("protocol/firmware mismatch"); + mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); + return -1; + } + + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); - _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators > 255) || - (_max_relays < 1) || (_max_relays > 255) || - (_max_transfer < 16) || (_max_transfer > 255) || + (_max_relays > 32) || + (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { - log("failed getting parameters from PX4IO"); - mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + log("config read error"); + mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; @@ -481,6 +604,7 @@ PX4IO::init() /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); + if (ret != OK) return ret; @@ -491,27 +615,31 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* get a status update from IO */ + io_get_status(); + + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_armed)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - - /* keep checking for an update, ensure we got a recent state, + + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); break; } @@ -519,7 +647,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -537,52 +665,65 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ - } while (!status.flag_system_armed); + /* re-send if necessary */ + if (!safety.armed) { + orb_publish(ORB_ID(vehicle_command), pub, &cmd); + log("re-sending arm cmd"); + } - /* regular boot, no in-air restart, init IO */ - } else { + /* keep waiting for state change for 2 s */ + } while (!safety.armed); + /* regular boot, no in-air restart, init IO */ + + } else { /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_FMU_ARMED | - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_FMU_ARMED | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); - /* publish RC config to IO */ - ret = io_set_rc_config(); - if (ret != OK) { - log("failed to update RC input config"); - mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); - return ret; + if (_rc_handling_disabled) { + ret = io_disable_rc_handling(); + + } else { + /* publish RC config to IO */ + ret = io_set_rc_config(); + + if (ret != OK) { + log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); + return ret; + } } } @@ -596,7 +737,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -617,11 +758,12 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - hrt_abstime last_poll_time = 0; - int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + hrt_abstime poll_last = 0; + hrt_abstime orb_check_last = 0; log("starting"); + _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -630,28 +772,26 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ - - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ - + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); _t_param = orb_subscribe(ORB_ID(parameter_update)); - orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + + if ((_t_actuators < 0) || + (_t_actuator_armed < 0) || + (_t_vehicle_control_mode < 0) || + (_t_param < 0) || + (_t_vehicle_command < 0)) { + log("subscription(s) failed"); + goto out; + } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[1]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; - fds[2].events = POLLIN; - fds[3].fd = _t_param; - fds[3].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -661,17 +801,19 @@ PX4IO::task_main() /* adjust update interval */ if (_update_interval != 0) { - if (_update_interval < 5) - _update_interval = 5; + if (_update_interval < UPDATE_INTERVAL_MIN) + _update_interval = UPDATE_INTERVAL_MIN; + if (_update_interval > 100) _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); _update_interval = 0; } /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + int ret = ::poll(fds, 1, 20); lock(); /* this would be bad... */ @@ -684,67 +826,79 @@ PX4IO::task_main() hrt_abstime now = hrt_absolute_time(); /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) + if (fds[0].revents & POLLIN) { io_set_control_state(); + } - /* if we have an arming state update, handle it */ - if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) - io_set_arming_state(); - - /* - * If it's time for another tick of the polling status machine, - * try it now. - */ - if ((now - last_poll_time) >= 20000) { + if (now >= poll_last + IO_POLL_INTERVAL) { + /* run at 50Hz */ + poll_last = now; - /* - * Pull status and alarms from IO. - */ + /* pull status and alarms from IO */ io_get_status(); - /* - * Get raw R/C input from IO. - */ + /* get raw R/C input from IO */ io_publish_raw_rc(); - /* - * Fetch mixed servo controls and PWM outputs from IO. - * - * XXX We could do this at a reduced rate in many/most cases. - */ + /* fetch mixed servo controls and PWM outputs from IO */ io_publish_mixed_controls(); io_publish_pwm_outputs(); + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* check updates on uORB topics and handle it */ + bool updated = false; + + /* arming state */ + orb_check(_t_actuator_armed, &updated); + + if (!updated) { + orb_check(_t_vehicle_control_mode, &updated); + } + + if (updated) { + io_set_arming_state(); + } + + /* vehicle command */ + orb_check(_t_vehicle_command, &updated); + + if (updated) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + + // Check for a DSM pairing command + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { + dsm_bind_ioctl((int)cmd.param2); + } + } /* * If parameters have changed, re-send RC mappings to IO * * XXX this may be a bit spammy */ - if (fds[3].revents & POLLIN) { + orb_check(_t_param, &updated); + + if (updated) { parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to 0 + /* see if bind parameter has been set, and reset it to -1 */ 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; + + if (dsm_bind_val > -1) { + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); } - /* copy to reset the notification */ - orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* re-upload RC input config as it may have changed */ io_set_rc_config(); } @@ -755,6 +909,7 @@ PX4IO::task_main() unlock(); +out: debug("exiting"); /* clean up the alternate device node */ @@ -774,7 +929,7 @@ PX4IO::io_set_control_state() /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + ORB_ID(actuator_controls_1), _t_actuators, &controls); for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); @@ -783,43 +938,36 @@ PX4IO::io_set_control_state() return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } -int -PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) -{ - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); -} int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } @@ -828,6 +976,21 @@ PX4IO::io_set_arming_state() } int +PX4IO::disable_rc_handling() +{ + return io_disable_rc_handling(); +} + +int +PX4IO::io_disable_rc_handling() +{ + uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; + uint16_t clear = 0; + + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); +} + +int PX4IO::io_set_rc_config() { unsigned offset = 0; @@ -849,22 +1012,27 @@ PX4IO::io_set_rc_config() * autopilots / GCS'. */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) input_map[i] = ichan++; @@ -919,6 +1087,7 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) { log("rc config upload failed"); break; @@ -945,14 +1114,15 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -967,9 +1137,50 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + safety.safety_off = true; + safety.safety_switch_available = true; + + } else { + safety.safety_off = false; + safety.safety_switch_available = true; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } +void +PX4IO::dsm_bind_ioctl(int dsmMode) +{ + if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* 0: dsm2, 1:dsmx */ + if ((dsmMode == 0) || (dsmMode == 1)) { + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); + } + + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + } +} + + int PX4IO::io_handle_alarms(uint16_t alarms) { @@ -983,53 +1194,89 @@ PX4IO::io_handle_alarms(uint16_t alarms) return 0; } +void +PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) +{ + /* only publish if battery has a valid minimum voltage */ + if (vbatt <= 3300) { + return; + } + + battery_status_s battery_status; + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = vbatt / 1000.0f; + + /* + ibatt contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; + + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } +} + +void +PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) +{ + servorail_status_s servorail_status; + servorail_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + servorail_status.voltage_v = vservo * 0.001f; + servorail_status.rssi_v = vrssi * 0.001f; + + /* lazily publish the servorail voltages */ + if (_to_servorail > 0) { + orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + + } else { + _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + } +} + int PX4IO::io_get_status() { - uint16_t regs[4]; + uint16_t regs[6]; int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + if (ret != OK) return ret; io_handle_status(regs[0]); io_handle_alarms(regs[1]); - /* only publish if battery has a valid minimum voltage */ - if (regs[2] > 3300) { - battery_status_s battery_status; - - battery_status.timestamp = hrt_absolute_time(); - - /* voltage is scaled to mV */ - battery_status.voltage_v = regs[2] / 1000.0f; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif - /* - regs[3] contains the raw ADC count, as 12 bit ADC - value, with full range being 3.3v - */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; - battery_status.current_a += _battery_amp_bias; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + io_handle_vservo(regs[4], regs[5]); +#endif - /* - integrate battery over time to get total mAh used - */ - if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; - } - battery_status.discharged_mah = _battery_mamphour_total; - _battery_last_timestamp = battery_status.timestamp; - - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } return ret; } @@ -1037,38 +1284,41 @@ int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) { uint32_t channel_count; - int ret = OK; + int ret; /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - + + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; + /* - * XXX Because the channel count and channel data are fetched - * separately, there is a risk of a race between the two - * that could leave us with channel data and a count that - * are out of sync. - * Fixing this would require a guarantee of atomicity from - * IO, and a single fetch for both count and channels. + * Read the channel count and the first 9 channels. * - * XXX Since IO has the input calibration info, we ought to be - * able to get the pre-fixed-up controls directly. - * - * XXX can we do this more cheaply? If we knew we had DMA, it would - * almost certainly be better to just get all the inputs... + * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - if (channel_count == _io_reg_get_error) - return -EIO; - if (channel_count > RC_INPUT_MAX_CHANNELS) - channel_count = RC_INPUT_MAX_CHANNELS; - input_rc.channel_count = channel_count; + input_rc.timestamp = hrt_absolute_time(); + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); - if (channel_count > 0) { - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); - if (ret == OK) - input_rc.timestamp = hrt_absolute_time(); + if (ret != OK) + return ret; + + /* + * Get the channel count any any extra channels. This is no more expensive than reading the + * channel count once. + */ + channel_count = regs[0]; + + if (channel_count > 9) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + + if (ret != OK) + return ret; } + input_rc.channel_count = channel_count; + memcpy(input_rc.values, ®s[prolog], channel_count * 2); + return ret; } @@ -1084,16 +1334,20 @@ PX4IO::io_publish_raw_rc() rc_val.timestamp = hrt_absolute_time(); int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) return ret; /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } @@ -1101,7 +1355,8 @@ PX4IO::io_publish_raw_rc() /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); - } else { + + } else { orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } @@ -1126,6 +1381,7 @@ PX4IO::io_publish_mixed_controls() /* get actuator controls from IO */ uint16_t act[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) return ret; @@ -1135,16 +1391,17 @@ PX4IO::io_publish_mixed_controls() /* laxily advertise on first publication */ if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); } return OK; @@ -1164,26 +1421,29 @@ PX4IO::io_publish_pwm_outputs() /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) return ret; /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) outputs.output[i] = ctl[i]; + outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ if (_to_outputs == 0) { _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + &outputs); + } else { orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); } return OK; @@ -1198,25 +1458,14 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_NORESTART; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + if (ret != (int)num_values) { + debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); + return -1; + } - /* perform the transfer */ - int ret = transfer(msgv, 2); - if (ret != OK) - debug("io_reg_set: error %d", ret); - return ret; + return OK; } int @@ -1228,25 +1477,20 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { - /* set up the transfer */ - uint8_t addr[2] = { - page, - offset - }; - i2c_msg_s msgv[2]; + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; + } - msgv[0].flags = 0; - msgv[0].buffer = addr; - msgv[0].length = 2; - msgv[1].flags = I2C_M_READ; - msgv[1].buffer = (uint8_t *)values; - msgv[1].length = num_values * sizeof(*values); + int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); - /* perform the transfer */ - int ret = transfer(msgv, 2); - if (ret != OK) - debug("io_reg_get: data error %d", ret); - return ret; + if (ret != (int)num_values) { + debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); + return -1; + } + + return OK; } uint32_t @@ -1254,7 +1498,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; - if (io_reg_get(page, offset, &value, 1)) + if (io_reg_get(page, offset, &value, 1) != OK) return _io_reg_get_error; return value; @@ -1267,8 +1511,10 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); - if (ret) + + if (ret != OK) return ret; + value &= ~clearbits; value |= setbits; @@ -1276,61 +1522,135 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int -PX4IO::mixer_send(const char *buf, unsigned buflen) +PX4IO::print_debug() { - uint8_t frame[_max_transfer]; - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + int io_fd = -1; - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; + if (io_fd < 0) { + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); + } - do { - unsigned count = buflen; + /* read IO's output */ + if (io_fd > 0) { + pollfd fds[1]; + fds[0].fd = io_fd; + fds[0].events = POLLIN; - if (count > max_len) - count = max_len; + usleep(500); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - } + if (pret > 0) { + int count; + char buf[65]; - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { - msg->text[count] = '\0'; - total_len++; - } + do { + count = ::read(io_fd, buf, sizeof(buf) - 1); - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + if (count > 0) { + /* enforce null termination */ + buf[count] = '\0'; + warnx("IO CONSOLE: %s", buf); + } - if (ret) { - log("mixer send error %d", ret); - return ret; + } while (count > 0); } - msg->action = F2I_MIXER_ACTION_APPEND; + ::close(io_fd); + return 0; + } + +#endif + return 1; + +} + +int +PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) +{ + /* get debug level */ + int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); + + uint8_t frame[_max_transfer]; - } while (buflen > 0); + do { + + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); + + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; + + do { + unsigned count = buflen; + + if (count > max_len) + count = max_len; + + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + + } else { + continue; + } + + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + + if (total_len % 2) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + /* print mixer chunk */ + if (debuglevel > 5 || ret) { + + warnx("fmu sent: \"%s\"", msg->text); + + /* read IO's output */ + print_debug(); + } + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + /* ensure a closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + retries--; + + log("mixer sent"); + + } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - debug("mixer upload OK"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; - } else { - debug("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } + log("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ return -EINVAL; } @@ -1339,120 +1659,162 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u software %u bootloader %u buffer %uB\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + printf("protocol %u hardware %u bootloader %u buffer %uB\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); /* status */ printf("%u bytes free\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + 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%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) && (!(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" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); + flags, + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), + ((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) && (!(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_PWM_PASSTHROUGH" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s%s\n", - alarms, - ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); - printf("vbatt %u ibatt %u vbatt scale %u\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); - printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", - (double)_battery_amp_per_volt, - (double)_battery_amp_bias, - (double)_battery_mamphour_total); + if (_hardware == 1) { + printf("vbatt mV %u ibatt mV %u vbatt scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE)); + printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n", + (double)_battery_amp_per_volt, + (double)_battery_amp_bias, + (double)_battery_mamphour_total); + + } else if (_hardware == 2) { + printf("vservo %u mV vservo scale %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); + printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); + } + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } + printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", - arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + printf("arming 0x%04x%s%s%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + printf("rates 0x%04x default %u alt %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); +#endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + + printf("\ndisarmed values"); + + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); + printf("\n"); } @@ -1484,38 +1846,131 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + /* get the default update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: { - - /* blindly clear the PWM update alarm - might be set for some other reason */ - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + case PWM_SERVO_GET_UPDATE_RATE: + /* get the alternative update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + break; - /* attempt to set the rate map */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + case PWM_SERVO_SET_SELECT_UPDATE_RATE: { - /* check that the changes took */ - uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { - ret = -EINVAL; + /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + + break; } + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + break; + + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); break; } + case PWM_SERVO_GET_FAILSAFE_PWM: + + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_DISARMED_PWM: + + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_MIN_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_MAX_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + 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); + usleep(72000); 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); @@ -1527,77 +1982,115 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { - /* TODO: we could go lower for e.g. TurboPWM */ - unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { - ret = -EINVAL; - } else { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - } + /* TODO: we could go lower for e.g. TurboPWM */ + unsigned channel = cmd - PWM_SERVO_SET(0); - break; - } + if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) { + ret = -EINVAL; + + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + } + + break; + } case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET(0); + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _max_actuators) { + ret = -EINVAL; - if (channel >= _max_actuators) { - ret = -EINVAL; - } else { - /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); - if (value == _io_reg_get_error) { - ret = -EIO; } else { - *(servo_position_t *)arg = value; + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); + + if (value == _io_reg_get_error) { + ret = -EIO; + + } else { + *(servo_position_t *)arg = value; + } } + + break; } - break; - } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - *(uint32_t *)arg = value; - break; - } + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; + + break; + } 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; - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + uint32_t bits = (1 << _max_relays) - 1; + + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif + break; + } case GPIO_SET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + } + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_CLEAR: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + break; + } + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_GET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; + +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case MIXERIOCGETOUTPUTCOUNT: @@ -1609,40 +2102,44 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - ret = mixer_send(buf, strnlen(buf, 2048)); - break; - } + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 2048)); + break; + } case RC_INPUT_GET: { - uint16_t status; - rc_input_values *rc_val = (rc_input_values *)arg; + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) - break; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - /* if no R/C input, don't try to fetch anything */ - if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { - ret = -ENOTCONN; - break; - } + if (ret != OK) + break; - /* sort out the source of the values */ - if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; - } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; - } + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } - /* read raw R/C input values */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); - break; - } + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); + break; + } case PX4IO_SET_DEBUG: /* set the debug level */ @@ -1650,12 +2147,15 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); } + break; default: @@ -1674,11 +2174,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) if (count > _max_actuators) count = _max_actuators; + if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) return ret; } + return count * 2; } @@ -1686,8 +2189,9 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 5) { - interval_ms = 5; + + if (interval_ms < UPDATE_INTERVAL_MIN) { + interval_ms = UPDATE_INTERVAL_MIN; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -1712,23 +2216,75 @@ extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { +device::Device * +get_interface() +{ + device::Device *interface = nullptr; + +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + + /* try for a serial interface */ + if (PX4IO_serial_interface != nullptr) + interface = PX4IO_serial_interface(); + + if (interface != nullptr) + goto got; + +#endif + + /* try for an I2C interface if we haven't got a serial one */ + if (PX4IO_i2c_interface != nullptr) + interface = PX4IO_i2c_interface(); + + if (interface != nullptr) + goto got; + + errx(1, "cannot alloc interface"); + +got: + + if (interface->init() != OK) { + delete interface; + errx(1, "interface init failed"); + } + + return interface; +} + void start(int argc, char *argv[]) { if (g_dev != nullptr) errx(1, "already loaded"); + /* allocate the interface */ + device::Device *interface = get_interface(); + /* create the driver - it will set g_dev */ - (void)new PX4IO(); + (void)new PX4IO(interface); if (g_dev == nullptr) errx(1, "driver alloc failed"); if (OK != g_dev->init()) { delete g_dev; + g_dev = nullptr; errx(1, "driver init failed"); } + /* disable RC handling on request */ + if (argc > 1) { + if (!strcmp(argv[1], "norc")) { + + if (g_dev->disable_rc_handling()) + warnx("Failed disabling RC handling"); + + } else { + warnx("unknown argument: %s", argv[1]); + } + } + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { @@ -1737,10 +2293,41 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } + +#endif exit(0); } void +detect(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(0, "already loaded"); + + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + int ret = g_dev->detect(); + + delete g_dev; + g_dev = nullptr; + + if (ret) { + /* nonzero, error */ + exit(1); + + } else { + exit(0); + } +} + +void bind(int argc, char *argv[]) { int pulses; @@ -1748,21 +2335,33 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); +#endif + if (argc < 3) errx(0, "needs argument, use dsm2 or dsmx"); if (!strcmp(argv[2], "dsm2")) - pulses = 3; + pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) - pulses = 7; + pulses = DSMX_BIND_PULSES; + else if (!strcmp(argv[2], "dsmx8")) + pulses = DSMX8_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + // Test for custom pulse parameter + if (argc > 3) + pulses = atoi(argv[3]); + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + errx(1, "system must not be armed"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - +#endif g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); @@ -1786,13 +2385,18 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -1800,22 +2404,27 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { if (pwm_value < 2000) { pwm_value++; + } else { direction = -1; } + } else { if (pwm_value > 1000) { pwm_value--; + } else { direction = 1; } @@ -1827,16 +2436,21 @@ test(void) if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) err(1, "error reading PWM servo %d", i); + if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } /* Check if user wants to quit */ char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); - close(console); exit(0); } } @@ -1846,33 +2460,54 @@ test(void) void monitor(void) { + /* clear screen */ + printf("\033[2J"); + unsigned cancels = 3; - printf("Hit <enter> three times to exit monitor mode\n"); for (;;) { pollfd fds[1]; fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 500); + poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); - if (cancels-- == 0) + if (cancels-- == 0) { + printf("\033[H"); /* move cursor home and clear screen */ exit(0); + } } -#warning implement this + if (g_dev != nullptr) { + + printf("\033[H"); /* move cursor home and clear screen */ + (void)g_dev->print_status(); + (void)g_dev->print_debug(); + printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); -// if (g_dev != nullptr) -// g_dev->dump_one = true; + } else { + errx(1, "driver not loaded, exiting"); + } } } +void +if_test(unsigned mode) +{ + device::Device *interface = get_interface(); + + int result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + + errx(0, "test returned %d", result); } +} /* namespace */ + int px4io_main(int argc, char *argv[]) { @@ -1883,102 +2518,131 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) start(argc - 1, argv + 1); - if (!strcmp(argv[1], "limit")) { + if (!strcmp(argv[1], "detect")) + detect(argc - 1, argv + 1); + + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { + printf("[px4io] loaded, detaching first\n"); + /* stop the driver */ + delete g_dev; + } - if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); - } else { - errx(1, "missing argument (50 - 200 Hz)"); - return 1; - } + PX4IO_Uploader *up; + const char *fn[3]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + + } else { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[1] = "/fs/microsd/px4io1.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = nullptr; +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[1] = "/fs/microsd/px4io2.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = nullptr; +#else +#error "unknown board" +#endif } - exit(0); - } - if (!strcmp(argv[1], "current")) { - if (g_dev != nullptr) { - if ((argc > 3)) { - g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); - } else { - errx(1, "missing argument (apm_per_volt, amp_offset)"); - return 1; - } + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + + case -ENOENT: + errx(1, "PX4IO firmware file not found"); + + case -EEXIST: + case -EIO: + errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + + case -EINVAL: + errx(1, "verify failed - retry the update"); + + case -ETIMEDOUT: + errx(1, "timed out waiting for bootloader - power-cycle and try again"); + + default: + errx(1, "unexpected error %d", ret); } - exit(0); + + return ret; } - if (!strcmp(argv[1], "failsafe")) { + if (!strcmp(argv[1], "iftest")) { + if (g_dev != nullptr) + errx(1, "can't iftest when started"); - if (argc < 3) { - errx(1, "failsafe command needs at least one channel value (ppm)"); - } + if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); + } - if (g_dev != nullptr) { + /* commands below here require a started driver */ - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; - - for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; - } - } + if (g_dev == nullptr) + errx(1, "not started"); - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + if (!strcmp(argv[1], "limit")) { + + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); - if (ret != OK) - errx(ret, "failed setting failsafe values"); } else { - errx(1, "not loaded"); + errx(1, "missing argument (50 - 500 Hz)"); + return 1; } + exit(0); } - if (!strcmp(argv[1], "recovery")) { + if (!strcmp(argv[1], "current")) { + if ((argc > 3)) { + g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); - if (g_dev != nullptr) { - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1); } else { - errx(1, "not loaded"); + errx(1, "missing argument (apm_per_volt, amp_offset)"); + return 1; } + + exit(0); + } + + + + if (!strcmp(argv[1], "recovery")) { + + /* + * Enable in-air restart support. + * 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); exit(0); } if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } + /* stop the driver */ + delete g_dev; exit(0); } if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } + printf("[px4io] loaded\n"); + g_dev->print_status(); exit(0); } @@ -1988,75 +2652,27 @@ px4io_main(int argc, char *argv[]) printf("usage: px4io debug LEVEL\n"); exit(1); } + if (g_dev == nullptr) { printf("px4io is not started\n"); exit(1); } + uint8_t level = atoi(argv[2]); /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); + if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); } + printf("SET_DEBUG %u OK\n", (unsigned)level); exit(0); } - /* note, stop not currently implemented */ - - if (!strcmp(argv[1], "update")) { - - if (g_dev != nullptr) { - printf("[px4io] loaded, detaching first\n"); - /* stop the driver */ - delete g_dev; - } - - PX4IO_Uploader *up; - const char *fn[3]; - - /* work out what we're uploading... */ - if (argc > 2) { - fn[0] = argv[2]; - fn[1] = nullptr; - - } else { - fn[0] = "/fs/microsd/px4io.bin"; - fn[1] = "/etc/px4io.bin"; - fn[2] = nullptr; - } - - up = new PX4IO_Uploader; - int ret = up->upload(&fn[0]); - delete up; - - switch (ret) { - case OK: - break; - - case -ENOENT: - errx(1, "PX4IO firmware file not found"); - - case -EEXIST: - case -EIO: - errx(1, "error updating PX4IO - check that bootloader mode is enabled"); - - case -EINVAL: - errx(1, "verify failed - retry the update"); - - case -ETIMEDOUT: - errx(1, "timed out waiting for bootloader - power-cycle and try again"); - - default: - errx(1, "unexpected error %d", ret); - } - - return ret; - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -2073,6 +2689,6 @@ px4io_main(int argc, char *argv[]) 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', 'bind', or 'update'"); +out: + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); } diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp new file mode 100644 index 000000000..19776c40a --- /dev/null +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -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 px4io_i2c.cpp + * + * I2C interface for PX4IO + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <errno.h> +#include <unistd.h> + +#include <arch/board/board.h> +#include <board_config.h> + +#include <drivers/device/i2c.h> + +#ifdef PX4_I2C_OBDEV_PX4IO + +device::Device *PX4IO_i2c_interface(); + +class PX4IO_I2C : public device::I2C +{ +public: + PX4IO_I2C(int bus, uint8_t address); + virtual ~PX4IO_I2C(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + +}; + +device::Device +*PX4IO_i2c_interface() +{ + return new PX4IO_I2C(PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO); +} + +PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : + I2C("PX4IO_i2c", nullptr, bus, address, 320000) +{ + _retries = 3; +} + +PX4IO_I2C::~PX4IO_I2C() +{ +} + +int +PX4IO_I2C::init() +{ + int ret; + + ret = I2C::init(); + if (ret != OK) + goto out; + + /* XXX really should do something more here */ + +out: + return 0; +} + +int +PX4IO_I2C::ioctl(unsigned operation, unsigned &arg) +{ + return 0; +} + +int +PX4IO_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = 2 * count; + + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; +} + +int +PX4IO_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = 2 * count; + + int ret = transfer(msgv, 2); + if (ret == OK) + ret = count; + return ret; +} + +#endif /* PX4_I2C_OBDEV_PX4IO */ diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp new file mode 100644 index 000000000..43318ca84 --- /dev/null +++ b/src/drivers/px4io/px4io_serial.cpp @@ -0,0 +1,679 @@ +/**************************************************************************** + * + * 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 px4io_serial.cpp + * + * Serial interface for PX4IO + */ + +/* XXX trim includes */ +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <nuttx/arch.h> +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> + +#include <debug.h> + +#include <drivers/device/device.h> +#include <drivers/drv_hrt.h> +#include <board_config.h> + +#include <systemlib/perf_counter.h> + +#include <modules/px4iofirmware/protocol.h> + +#ifdef PX4IO_SERIAL_BASE + +device::Device *PX4IO_serial_interface(); + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +class PX4IO_serial : public device::Device +{ +public: + PX4IO_serial(); + virtual ~PX4IO_serial(); + + virtual int init(); + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + virtual int ioctl(unsigned operation, unsigned &arg); + +private: + /* + * XXX tune this value + * + * At 1.5Mbps each register takes 13.3µs, and we always transfer a full packet. + * Packet overhead is 26µs for the four-byte header. + * + * 32 registers = 451µs + * + * Maybe we can just send smaller packets (e.g. 8 regs) and loop for larger (less common) + * transfers? Could cause issues with any regs expecting to be written atomically... + */ + static IOPacket _dma_buffer; // XXX static to ensure DMA-able memory + + DMA_HANDLE _tx_dma; + DMA_HANDLE _rx_dma; + + /** saved DMA status */ + static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values + static const unsigned _dma_status_waiting = 0x00000000; + volatile unsigned _rx_dma_status; + + /** bus-ownership lock */ + sem_t _bus_semaphore; + + /** client-waiting lock/signal */ + sem_t _completion_semaphore; + + /** + * Start the transaction with IO and wait for it to complete. + */ + int _wait_complete(); + + /** + * DMA completion handler. + */ + static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); + void _do_rx_dma_callback(unsigned status); + + /** + * Serial interrupt handler. + */ + static int _interrupt(int vector, void *context); + void _do_interrupt(); + + /** + * Cancel any DMA in progress with an error. + */ + void _abort_dma(); + + /** + * Performance counters. + */ + perf_counter_t _pc_txns; + perf_counter_t _pc_dmasetup; + perf_counter_t _pc_retries; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_dmaerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; + +}; + +IOPacket PX4IO_serial::_dma_buffer; +static PX4IO_serial *g_interface; + +device::Device +*PX4IO_serial_interface() +{ + return new PX4IO_serial(); +} + +PX4IO_serial::PX4IO_serial() : + Device("PX4IO_serial"), + _tx_dma(nullptr), + _rx_dma(nullptr), + _rx_dma_status(_dma_status_inactive), + _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), + _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), + _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), + _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), + _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), + _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")), + _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), + _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), + _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) +{ + g_interface = this; +} + +PX4IO_serial::~PX4IO_serial() +{ + if (_tx_dma != nullptr) { + stm32_dmastop(_tx_dma); + stm32_dmafree(_tx_dma); + } + if (_rx_dma != nullptr) { + stm32_dmastop(_rx_dma); + stm32_dmafree(_rx_dma); + } + + /* reset the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + + /* restore the GPIOs */ + stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO); + stm32_unconfiggpio(PX4IO_SERIAL_RX_GPIO); + + /* and kill our semaphores */ + sem_destroy(&_completion_semaphore); + sem_destroy(&_bus_semaphore); + + perf_free(_pc_txns); + perf_free(_pc_dmasetup); + perf_free(_pc_retries); + perf_free(_pc_timeouts); + perf_free(_pc_crcerrs); + perf_free(_pc_dmaerrs); + perf_free(_pc_protoerrs); + perf_free(_pc_uerrs); + perf_free(_pc_idle); + perf_free(_pc_badidle); + + if (g_interface == this) + g_interface = nullptr; +} + +int +PX4IO_serial::init() +{ + /* allocate DMA */ + _tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP); + _rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP); + if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) + return -1; + + /* configure pins for serial use */ + stm32_configgpio(PX4IO_SERIAL_TX_GPIO); + stm32_configgpio(PX4IO_SERIAL_RX_GPIO); + + /* reset & configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* eat any existing interrupt status */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* attach serial interrupt handler */ + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* enable UART in DMA mode, enable error and line idle interrupts */ + rCR3 = USART_CR3_EIE; + + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + + /* create semaphores */ + sem_init(&_completion_semaphore, 0, 0); + sem_init(&_bus_semaphore, 0, 1); + + + /* XXX this could try talking to IO */ + + return 0; +} + +int +PX4IO_serial::ioctl(unsigned operation, unsigned &arg) +{ + + switch (operation) { + + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + lowsyslog("test 0\n"); + + /* kill DMA, this is a PIO test */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); + rCR3 &= ~(USART_CR3_DMAR | USART_CR3_DMAT); + + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0x55; + } + return 0; + + case 1: + { + unsigned fails = 0; + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) + fails++; + + if (count >= 5000) { + lowsyslog("==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_dmasetup); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_dmaerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + perf_print_counter(_pc_idle); + perf_print_counter(_pc_badidle); + count = 0; + } + } + return 0; + } + case 2: + lowsyslog("test 2\n"); + return 0; + } + default: + break; + } + + return -1; +} + +int +PX4IO_serial::write(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + const uint16_t *values = reinterpret_cast<const uint16_t *>(data); + + if (count > PKT_MAX_REGS) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = count | PKT_CODE_WRITE; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count)); + for (unsigned i = count; i < PKT_MAX_REGS; i++) + _dma_buffer.regs[i] = 0x55aa; + + /* XXX implement check byte */ + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + result = -EINVAL; + perf_count(_pc_protoerrs); + } + + break; + } + perf_count(_pc_retries); + } + + sem_post(&_bus_semaphore); + + if (result == OK) + result = count; + return result; +} + +int +PX4IO_serial::read(unsigned address, void *data, unsigned count) +{ + uint8_t page = address >> 8; + uint8_t offset = address & 0xff; + uint16_t *values = reinterpret_cast<uint16_t *>(data); + + if (count > PKT_MAX_REGS) + return -EINVAL; + + sem_wait(&_bus_semaphore); + + int result; + for (unsigned retries = 0; retries < 3; retries++) { + + _dma_buffer.count_code = count | PKT_CODE_READ; + _dma_buffer.page = page; + _dma_buffer.offset = offset; + + /* start the transaction and wait for it to complete */ + result = _wait_complete(); + + /* successful transaction? */ + if (result == OK) { + + /* check result in packet */ + if (PKT_CODE(_dma_buffer) == PKT_CODE_ERROR) { + + /* IO didn't like it - no point retrying */ + result = -EINVAL; + perf_count(_pc_protoerrs); + + /* compare the received count with the expected count */ + } else if (PKT_COUNT(_dma_buffer) != count) { + + /* IO returned the wrong number of registers - no point retrying */ + result = -EIO; + perf_count(_pc_protoerrs); + + /* successful read */ + } else { + + /* copy back the result */ + memcpy(values, &_dma_buffer.regs[0], (2 * count)); + } + + break; + } + perf_count(_pc_retries); + } + + sem_post(&_bus_semaphore); + + if (result == OK) + result = count; + return result; +} + +int +PX4IO_serial::_wait_complete() +{ + /* clear any lingering error status */ + (void)rSR; + (void)rDR; + + /* start RX DMA */ + perf_begin(_pc_txns); + perf_begin(_pc_dmasetup); + + /* DMA setup time ~3µs */ + _rx_dma_status = _dma_status_waiting; + + /* + * Note that we enable circular buffer mode as a workaround for + * there being no API to disable the DMA FIFO. We need direct mode + * because otherwise when the line idle interrupt fires there + * will be packet bytes still in the DMA FIFO, and we will assume + * that the idle was spurious. + * + * XXX this should be fixed with a NuttX change. + */ + stm32_dmasetup( + _rx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast<uint32_t>(&_dma_buffer), + sizeof(_dma_buffer), + DMA_SCR_CIRC | /* XXX see note above */ + DMA_SCR_DIR_P2M | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_rx_dma, _dma_callback, this, false); + rCR3 |= USART_CR3_DMAR; + + /* start TX DMA - no callback if we also expect a reply */ + /* DMA setup time ~3µs */ + _dma_buffer.crc = 0; + _dma_buffer.crc = crc_packet(&_dma_buffer); + stm32_dmasetup( + _tx_dma, + PX4IO_SERIAL_BASE + STM32_USART_DR_OFFSET, + reinterpret_cast<uint32_t>(&_dma_buffer), + PKT_SIZE(_dma_buffer), + DMA_SCR_DIR_M2P | + DMA_SCR_MINC | + DMA_SCR_PSIZE_8BITS | + DMA_SCR_MSIZE_8BITS | + DMA_SCR_PBURST_SINGLE | + DMA_SCR_MBURST_SINGLE); + stm32_dmastart(_tx_dma, nullptr, nullptr, false); + //rCR1 &= ~USART_CR1_TE; + //rCR1 |= USART_CR1_TE; + rCR3 |= USART_CR3_DMAT; + + perf_end(_pc_dmasetup); + + /* compute the deadline for a 10ms timeout */ + struct timespec abstime; + clock_gettime(CLOCK_REALTIME, &abstime); + abstime.tv_nsec += 10*1000*1000; + if (abstime.tv_nsec >= 1000*1000*1000) { + abstime.tv_sec++; + abstime.tv_nsec -= 1000*1000*1000; + } + + /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ + int ret; + for (;;) { + ret = sem_timedwait(&_completion_semaphore, &abstime); + + if (ret == OK) { + /* check for DMA errors */ + if (_rx_dma_status & DMA_STATUS_TEIF) { + perf_count(_pc_dmaerrs); + ret = -EIO; + break; + } + + /* check packet CRC - corrupt packet errors mean IO receive CRC error */ + uint8_t crc = _dma_buffer.crc; + _dma_buffer.crc = 0; + if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) { + perf_count(_pc_crcerrs); + ret = -EIO; + break; + } + + /* successful txn (may still be reporting an error) */ + break; + } + + if (errno == ETIMEDOUT) { + /* something has broken - clear out any partial DMA state and reconfigure */ + _abort_dma(); + perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ + break; + } + + /* we might? see this for EINTR */ + lowsyslog("unexpected ret %d/%d\n", ret, errno); + } + + /* reset DMA status */ + _rx_dma_status = _dma_status_inactive; + + /* update counters */ + perf_end(_pc_txns); + + return ret; +} + +void +PX4IO_serial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + if (arg != nullptr) { + PX4IO_serial *ps = reinterpret_cast<PX4IO_serial *>(arg); + + ps->_do_rx_dma_callback(status); + } +} + +void +PX4IO_serial::_do_rx_dma_callback(unsigned status) +{ + /* on completion of a reply, wake the waiter */ + if (_rx_dma_status == _dma_status_waiting) { + + /* check for packet overrun - this will occur after DMA completes */ + uint32_t sr = rSR; + if (sr & (USART_SR_ORE | USART_SR_RXNE)) { + (void)rDR; + status = DMA_STATUS_TEIF; + } + + /* save RX status */ + _rx_dma_status = status; + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* complete now */ + sem_post(&_completion_semaphore); + } +} + +int +PX4IO_serial::_interrupt(int irq, void *context) +{ + if (g_interface != nullptr) + g_interface->_do_interrupt(); + return 0; +} + +void +PX4IO_serial::_do_interrupt() +{ + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* read DR to clear status */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + /* + * If we are in the process of listening for something, these are all fatal; + * abort the DMA with an error. + */ + if (_rx_dma_status == _dma_status_waiting) { + _abort_dma(); + + perf_count(_pc_uerrs); + /* complete DMA as though in error */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + + return; + } + + /* XXX we might want to use FE / line break as an out-of-band handshake ... handle it here */ + + /* don't attempt to handle IDLE if it's set - things went bad */ + return; + } + + if (sr & USART_SR_IDLE) { + + /* if there is DMA reception going on, this is a short packet */ + if (_rx_dma_status == _dma_status_waiting) { + + /* verify that the received packet is complete */ + unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { + perf_count(_pc_badidle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TEIF); + return; + } + + perf_count(_pc_idle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TCIF); + } + } +} + +void +PX4IO_serial::_abort_dma() +{ + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* stop DMA */ + stm32_dmastop(_tx_dma); + stm32_dmastop(_rx_dma); +} + +#endif /* PX4IO_SERIAL_BASE */ diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 9e3f041e3..d01dedb0d 100644 --- a/src/drivers/px4io/uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -52,51 +52,14 @@ #include <termios.h> #include <sys/stat.h> +#include <crc32.h> + #include "uploader.h" -static const uint32_t crctab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -static uint32_t -crc32(const uint8_t *src, unsigned len, unsigned state) -{ - for (unsigned i = 0; i < len; i++) - state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); - return state; -} +#include <board_config.h> + +// define for comms logging +//#define UDEBUG PX4IO_Uploader::PX4IO_Uploader() : _io_fd(-1), @@ -115,13 +78,42 @@ PX4IO_Uploader::upload(const char *filenames[]) const char *filename = NULL; size_t fw_size; - _io_fd = open("/dev/ttyS2", O_RDWR); +#ifndef PX4IO_SERIAL_DEVICE +#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload +#endif + + /* allow an early abort and look for file first */ + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + + log("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + log("no firmware found"); + close(_io_fd); + _io_fd = -1; + return -ENOENT; + } + + _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR); if (_io_fd < 0) { log("could not open interface"); return -errno; } + /* save initial uart configuration to reset after the update */ + struct termios t_original; + tcgetattr(_io_fd, &t_original); + /* adjust line speed to match bootloader */ struct termios t; tcgetattr(_io_fd, &t); @@ -134,34 +126,16 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; } - for (unsigned i = 0; filenames[i] != nullptr; i++) { - _fw_fd = open(filenames[i], O_RDONLY); - - if (_fw_fd < 0) { - log("failed to open %s", filenames[i]); - continue; - } - - log("using firmware from %s", filenames[i]); - filename = filenames[i]; - break; - } - - if (filename == NULL) { - log("no firmware found"); - close(_io_fd); - _io_fd = -1; - return -ENOENT; - } - struct stat st; if (stat(filename, &st) != 0) { log("Failed to stat %s - %d\n", filename, (int)errno); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -errno; @@ -169,6 +143,7 @@ PX4IO_Uploader::upload(const char *filenames[]) fw_size = st.st_size; if (_fw_fd == -1) { + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -ENOENT; @@ -183,6 +158,7 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { /* this is immediately fatal */ log("bootloader not responding"); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return -EIO; @@ -196,6 +172,7 @@ PX4IO_Uploader::upload(const char *filenames[]) log("found bootloader revision: %d", bl_rev); } else { log("found unsupported bootloader revision %d, exiting", bl_rev); + tcsetattr(_io_fd, TCSANOW, &t_original); close(_io_fd); _io_fd = -1; return OK; @@ -231,6 +208,9 @@ PX4IO_Uploader::upload(const char *filenames[]) if (ret != OK) { log("reboot failed"); + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_io_fd); + _io_fd = -1; return ret; } @@ -240,6 +220,9 @@ PX4IO_Uploader::upload(const char *filenames[]) break; } + /* reset uart to previous/default baudrate */ + tcsetattr(_io_fd, TCSANOW, &t_original); + close(_fw_fd); close(_io_fd); _io_fd = -1; @@ -254,16 +237,20 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) fds[0].fd = _io_fd; fds[0].events = POLLIN; - /* wait 100 ms for a character */ + /* wait <timout> ms for a character */ int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { - //log("poll timeout %d", ret); +#ifdef UDEBUG + log("poll timeout %d", ret); +#endif return -ETIMEDOUT; } read(_io_fd, &c, 1); - //log("recv 0x%02x", c); +#ifdef UDEBUG + log("recv 0x%02x", c); +#endif return OK; } @@ -289,16 +276,20 @@ PX4IO_Uploader::drain() do { ret = recv(c, 1000); +#ifdef UDEBUG if (ret == OK) { - //log("discard 0x%02x", c); + log("discard 0x%02x", c); } +#endif } while (ret == OK); } int PX4IO_Uploader::send(uint8_t c) { - //log("send 0x%02x", c); +#ifdef UDEBUG + log("send 0x%02x", c); +#endif if (write(_io_fd, &c, 1) != 1) return -errno; @@ -572,14 +563,14 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local) return -errno; /* calculate crc32 sum */ - sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum); + sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum); bytes_read += count; } /* fill the rest with 0xff */ while (bytes_read < fw_size_remote) { - sum = crc32(&fill_blank, sizeof(fill_blank), sum); + sum = crc32part(&fill_blank, sizeof(fill_blank), sum); bytes_read += sizeof(fill_blank); } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 135202dd1..22387a3e2 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -69,7 +69,7 @@ private: PROTO_REBOOT = 0x30, INFO_BL_REV = 1, /**< bootloader protocol revision */ - BL_REV = 3, /**< supported bootloader protocol */ + BL_REV = 4, /**< supported bootloader protocol */ INFO_BOARD_ID = 2, /**< board type */ INFO_BOARD_REV = 3, /**< board revision */ INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk new file mode 100644 index 000000000..39b53ec9e --- /dev/null +++ b/src/drivers/rgbled/module.mk @@ -0,0 +1,6 @@ +# +# TCA62724FMG driver for RGB LED +# + +MODULE_COMMAND = rgbled +SRCS = rgbled.cpp diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp new file mode 100644 index 000000000..727c86e02 --- /dev/null +++ b/src/drivers/rgbled/rgbled.cpp @@ -0,0 +1,707 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: 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 + * 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 rgbled.cpp + * + * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. + * + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> + +#include <nuttx/wqueue.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <board_config.h> + +#include <drivers/drv_rgbled.h> + +#define RGBLED_ONTIME 120 +#define RGBLED_OFFTIME 120 + +#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */ +#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ +#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */ +#define SUB_ADDR_PWM1 0x82 /**< green (without auto-increment) */ +#define SUB_ADDR_PWM2 0x83 /**< red (without auto-increment) */ +#define SUB_ADDR_SETTINGS 0x84 /**< settings (without auto-increment)*/ + +#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ +#define SETTING_ENABLE 0x02 /**< on */ + + +class RGBLED : public device::I2C +{ +public: + RGBLED(int bus, int rgbled); + virtual ~RGBLED(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + +private: + work_s _work; + + rgbled_mode_t _mode; + rgbled_pattern_t _pattern; + + uint8_t _r; + uint8_t _g; + uint8_t _b; + float _brightness; + + bool _running; + int _led_interval; + bool _should_run; + int _counter; + + void set_color(rgbled_color_t ledcolor); + void set_mode(rgbled_mode_t mode); + void set_pattern(rgbled_pattern_t *pattern); + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); +}; + +/* for now, we only support one RGBLED */ +namespace +{ +RGBLED *g_rgbled; +} + +void rgbled_usage(); + +extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); + +RGBLED::RGBLED(int bus, int rgbled) : + I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), + _mode(RGBLED_MODE_OFF), + _r(0), + _g(0), + _b(0), + _brightness(1.0f), + _running(false), + _led_interval(0), + _should_run(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); + memset(&_pattern, 0, sizeof(_pattern)); +} + +RGBLED::~RGBLED() +{ +} + +int +RGBLED::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); + + return OK; +} + +int +RGBLED::probe() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } + + return ret; +} + +int +RGBLED::info() +{ + int ret; + bool on, powersave; + uint8_t r, g, b; + + ret = get(on, powersave, r, g, b); + + if (ret == OK) { + /* we don't care about power-save mode */ + log("state: %s", on ? "ON" : "OFF"); + log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + + } else { + warnx("failed to read led"); + } + + return ret; +} + +int +RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case RGBLED_SET_RGB: + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); + return OK; + + case RGBLED_SET_COLOR: + /* set the specified color name */ + set_color((rgbled_color_t)arg); + send_led_rgb(); + return OK; + + case RGBLED_SET_MODE: + /* set the specified mode */ + set_mode((rgbled_mode_t)arg); + return OK; + + case RGBLED_SET_PATTERN: + /* set a special pattern */ + set_pattern((rgbled_pattern_t *)arg); + return OK; + + default: + break; + } + + return ret; +} + + +void +RGBLED::led_trampoline(void *arg) +{ + RGBLED *rgbl = reinterpret_cast<RGBLED *>(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +RGBLED::led() +{ + if (!_should_run) { + _running = false; + return; + } + + switch (_mode) { + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) + _counter = 0; + + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) + _counter = 0; + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + break; + + case RGBLED_MODE_PATTERN: + + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + _counter = 0; + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; + } + + _counter++; + + /* re-queue ourselves to run again later */ + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); +} + +/** + * Parse color constant and set _r _g _b values + */ +void +RGBLED::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + warnx("color unknown"); + break; + } +} + +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ +void +RGBLED::set_mode(rgbled_mode_t mode) +{ + if (mode != _mode) { + _mode = mode; + + switch (mode) { + case RGBLED_MODE_OFF: + _should_run = false; + send_led_enable(false); + break; + + case RGBLED_MODE_ON: + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); + break; + + case RGBLED_MODE_BLINK_SLOW: + _should_run = true; + _counter = 0; + _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_NORMAL: + _should_run = true; + _counter = 0; + _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BLINK_FAST: + _should_run = true; + _counter = 0; + _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); + break; + + case RGBLED_MODE_BREATHE: + _should_run = true; + _counter = 0; + _led_interval = 25; + send_led_enable(true); + break; + + case RGBLED_MODE_PATTERN: + _should_run = true; + _counter = 0; + _brightness = 1.0f; + send_led_enable(true); + break; + + default: + warnx("mode unknown"); + break; + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } + + } +} + +/** + * Set pattern for PATTERN mode, but don't change current mode + */ +void +RGBLED::set_pattern(rgbled_pattern_t *pattern) +{ + memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +RGBLED::send_led_enable(bool enable) +{ + uint8_t settings_byte = 0; + + if (enable) + settings_byte |= SETTING_ENABLE; + + settings_byte |= SETTING_NOT_POWERSAVE; + + const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; + + return transfer(msg, sizeof(msg), nullptr, 0); +} + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED::send_led_rgb() +{ + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), + SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), + SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + }; + return transfer(msg, sizeof(msg), nullptr, 0); +} + +int +RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) +{ + uint8_t result[2]; + int ret; + + ret = transfer(nullptr, 0, &result[0], 2); + + if (ret == OK) { + on = result[0] & SETTING_ENABLE; + powersave = !(result[0] & SETTING_NOT_POWERSAVE); + /* XXX check, looks wrong */ + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; + } + + return ret; +} + +void +rgbled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +rgbled_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int rgbledadr = ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + rgbledadr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + rgbled_usage(); + exit(0); + } + } + + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_rgbled != nullptr) + errx(1, "already started"); + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + + if (g_rgbled != nullptr && OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + } + + if (g_rgbled == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_rgbled == nullptr) { + g_rgbled = new RGBLED(i2cdevice, rgbledadr); + + if (g_rgbled == nullptr) + errx(1, "new failed"); + + if (OK != g_rgbled->init()) { + delete g_rgbled; + g_rgbled = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_rgbled == nullptr) { + warnx("not started"); + rgbled_usage(); + exit(0); + } + + if (!strcmp(verb, "test")) { + fd = open(RGBLED_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; + + ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_rgbled->info(); + exit(0); + } + + if (!strcmp(verb, "off")) { + fd = open(RGBLED_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: rgbled rgb <red> <green> <blue>"); + } + + fd = open(RGBLED_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " RGBLED_DEVICE_PATH); + } + + rgbled_rgbset_t v; + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); + ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); + close(fd); + exit(ret); + } + + rgbled_usage(); + exit(0); +} diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000..d65a9be36 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * 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 RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include <poll.h> +#include <stdio.h> +#include <math.h> +#include <string.h> +#include <fcntl.h> +#include <termios.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> + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): + _address(address), + _pulsesPerRev(pulsesPerRev), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor1Overflow(0), + _motor2Position(0), + _motor2Speed(0), + _motor2Overflow(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; + struct termios uart_config; + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i<nread;i++) { + //printf("%d\t", rbuf[i]); + //} + //printf("\n"); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + uint8_t checksum = rbuf[5]; + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + checksum_mask; + // check if checksum is valid + if (checksum != checksum_computed) { + printf("checksum failed: expected %d got %d\n", + checksum, checksum_computed); + return -1; + } + int overFlow = 0; + + if (status & STATUS_REVERSE) { + //printf("roboclaw: reverse\n"); + } + + if (status & STATUS_UNDERFLOW) { + //printf("roboclaw: underflow\n"); + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + //printf("roboclaw: overflow\n"); + overFlow = +1; + } + + static int64_t overflowAmount = 0x100000000LL; + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + _motor1Position = float(int64_t(count) + + _motor1Overflow*overflowAmount)/_pulsesPerRev; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + _motor2Position = float(int64_t(count) + + _motor2Overflow*overflowAmount)/_pulsesPerRev; + } + return 0; +} + +void RoboClaw::printStatus(char *string, size_t n) +{ + snprintf(string, n, + "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } + return -1; +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } + return -1; +} + +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + +int RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } + return 0; +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + //printf("sum\n"); + for (size_t i=0;i<n;i++) { + sum += buf[i]; + //printf("%d\t", buf[i]); + } + //printf("total sum %d\n", sum); + return sum; +} + +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, + size_t n_data, uint16_t & prev_sum) +{ + tcflush(_uart, TCIOFLUSH); // flush buffers + uint8_t buf[n_data + 3]; + buf[0] = _address; + buf[1] = cmd; + for (size_t i=0;i<n_data;i++) { + buf[i+2] = data[n_data - i - 1]; // MSB + } + uint16_t sum = _sumBytes(buf, n_data + 2); + prev_sum += sum; + buf[n_data + 2] = sum & checksum_mask; + //printf("\nmessage:\n"); + //for (size_t i=0;i<n_data+3;i++) { + //printf("%d\t", buf[i]); + //} + //printf("\n"); + return write(_uart, buf, n_data + 3); +} + +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev) +{ + printf("roboclaw test: starting\n"); + + // setup + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3); + char buf[200]; + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3); + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + printf("Test complete\n"); + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp new file mode 100644 index 000000000..e9f35cf95 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * 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 RoboClas.hpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#pragma once + +#include <poll.h> +#include <stdio.h> +#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/topics/actuator_controls.h> +#include <drivers/device/i2c.h> + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + * @param pulsesPerRev # of encoder + * pulses per revolution of wheel + */ + RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + int update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void printStatus(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + uint16_t _pulsesPerRev; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription<actuator_controls_s> _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 000000000..1abecf198 --- /dev/null +++ b/src/drivers/roboclaw/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. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000..44ed03254 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * 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 roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include <arch/board/board.h> +#include "RoboClaw.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 roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * 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 roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + const char * deviceName = "/dev/ttyS2"; + uint8_t address = 128; + uint16_t pulsesPerRev = 1200; + + if (argc == 2) { + printf("testing with default settings\n"); + } else if (argc != 4) { + printf("usage: roboclaw test device address pulses_per_rev\n"); + exit(-1); + } else { + deviceName = argv[2]; + address = strtoul(argv[3], nullptr, 0); + pulsesPerRev = strtoul(argv[4], nullptr, 0); + } + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + roboclawTest(deviceName, address, pulsesPerRev); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0); + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + // start + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 1020eb946..00e46d6b8 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -227,7 +227,6 @@ ADC::init() if ((hrt_absolute_time() - now) > 500) { log("sample timeout"); return -1; - return 0xffff; } } @@ -282,7 +281,7 @@ ADC::close_last(struct file *filp) void ADC::_tick_trampoline(void *arg) { - ((ADC *)arg)->_tick(); + (reinterpret_cast<ADC *>(arg))->_tick(); } void @@ -342,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[8]; + adc_msg_s data[10]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) @@ -366,8 +365,15 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for PX4FMU - should be configurable */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* XXX this hardcodes the default channel set for PX4FMUv1 - should be configurable */ g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + /* XXX this hardcodes the default channel set for PX4FMUv2 - should be configurable */ + g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | + (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 83a1a1abb..e79d7e10a 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -59,7 +59,7 @@ #include <errno.h> #include <string.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include "chip.h" diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 2284be84d..f36f2091e 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -104,7 +104,7 @@ #include <math.h> #include <ctype.h> -#include <arch/board/board.h> +#include <board_config.h> #include <drivers/drv_hrt.h> #include <arch/stm32/chip.h> @@ -230,13 +230,18 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + inline const char *name(int tune) { + return _tune_names[tune]; + } private: - static const unsigned _tune_max = 1024; // be reasonable about user tunes - static const char * const _default_tunes[]; - static const unsigned _default_ntunes; + static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes + const char * _default_tunes[TONE_NUMBER_OF_TUNES]; + const char * _tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + const char *_user_tune; const char *_tune; // current tune string @@ -302,160 +307,6 @@ private: }; -// predefined tune array -const char * const ToneAlarm::_default_tunes[] = { - "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune - "MBT200a8a8a8PaaaP", // ERROR tone - "MFT200e8a8a", // NotifyPositive tone - "MFT200e8e", // NotifyNeutral tone - "MFT200e8c8e8c8e8c8", // NotifyNegative tone - "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! - "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie - "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha - "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee - "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy - "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell - "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" - "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" - "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" - "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" - "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" - "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" - "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" - "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" - "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" - "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" - "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" - "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" - "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" - "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" - "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" - "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" - "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" - "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" - "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" - "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" - "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" - "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" - "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" - "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" - "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" - "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" - "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" - "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" - "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" - "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" - "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" - "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" - "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" - "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" - "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" - "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" - "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" - "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" - "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" - "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" - "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" - "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" - "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" - "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" - "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" - "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" - "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" - "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" - "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" - "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" - "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" - "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" - "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" - "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" - "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." - "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" - "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" - "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" - "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" - "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" - "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" - "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" - "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" - "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" - "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" - "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" - "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" - "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" - "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" - "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" - "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" - "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" - "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" - "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" - "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" - "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" - "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" - "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" - "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" - "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" - "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" - "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" - "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" - "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" - "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" - "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" - "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" - "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" - "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" - "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" - "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" - "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" - "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" - "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" - "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" - "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" - "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" - "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" - "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" - "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" - "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" - "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" - "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" - "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" - "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" - "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" - "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" - "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" - "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" - "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" - "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" - "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" - "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" - "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" - "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" - "O2E2P64", -}; - -const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); - // semitone offsets from C for the characters 'A'-'G' const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; @@ -466,13 +317,33 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev("tone_alarm", "/dev/tone_alarm"), + CDev("tone_alarm", TONEALARM_DEVICE_PATH), + _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), _next(nullptr) { // enable debug() calls //_debug_enabled = true; + _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune + _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone + _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone + _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone + _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone + _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning + _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow + _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + + _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune + _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone + _tune_names[TONE_NOTIFY_POSITIVE_TUNE] = "positive"; // Notify Positive tone + _tune_names[TONE_NOTIFY_NEUTRAL_TUNE] = "neutral"; // Notify Neutral tone + _tune_names[TONE_NOTIFY_NEGATIVE_TUNE] = "negative"; // Notify Negative tone + _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning + _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow + _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast + _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning } ToneAlarm::~ToneAlarm() @@ -799,8 +670,12 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) + if (_repeat) { start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } return; } @@ -863,14 +738,20 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); - if (arg <= _default_ntunes) { - if (arg == 0) { + if (arg < TONE_NUMBER_OF_TUNES) { + if (arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; + _repeat = false; + _default_tune_number = 0; } else { - // play the selected tune - start_tune(_default_tunes[arg - 1]); + /* always interrupt alarms, unless they are repeating and already playing */ + if (!(_repeat && _default_tune_number == arg)) { + /* play the selected tune */ + _default_tune_number = arg; + start_tune(_default_tunes[arg]); + } } } else { result = -EINVAL; @@ -941,10 +822,10 @@ play_tune(unsigned tune) { int fd, ret; - fd = open("/dev/tone_alarm", 0); + fd = open(TONEALARM_DEVICE_PATH, 0); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); @@ -956,18 +837,21 @@ play_tune(unsigned tune) } int -play_string(const char *str) +play_string(const char *str, bool free_buffer) { int fd, ret; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = write(fd, str, strlen(str) + 1); close(fd); + if (free_buffer) + free((void *)str); + if (ret < 0) err(1, "play tune"); exit(0); @@ -993,22 +877,50 @@ tone_alarm_main(int argc, char *argv[]) } } - if ((argc > 1) && !strcmp(argv[1], "start")) - play_tune(1); - - if ((argc > 1) && !strcmp(argv[1], "stop")) - play_tune(0); - - if ((tune = strtol(argv[1], nullptr, 10)) != 0) - play_tune(tune); + if (argc > 1) { + const char *argv1 = argv[1]; + + if (!strcmp(argv1, "start")) + play_tune(TONE_STARTUP_TUNE); + + if (!strcmp(argv1, "stop")) + play_tune(TONE_STOP_TUNE); + + if ((tune = strtol(argv1, nullptr, 10)) != 0) + play_tune(tune); + + /* It might be a tune name */ + for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) + if (!strcmp(g_dev->name(tune), argv1)) + play_tune(tune); + + /* If it is a file name then load and play it as a string */ + if (*argv1 == '/') { + FILE *fd = fopen(argv1, "r"); + int sz; + char *buffer; + if (fd == nullptr) + errx(1, "couldn't open '%s'", argv1); + fseek(fd, 0, SEEK_END); + sz = ftell(fd); + fseek(fd, 0, SEEK_SET); + buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) + errx(1, "not enough memory memory"); + fread(buffer, sz, 1, fd); + /* terminate the string */ + buffer[sz] = 0; + play_string(buffer, true); + } - /* if it looks like a PLAY string... */ - if (strlen(argv[1]) > 2) { - const char *str = argv[1]; - if (str[0] == 'M') { - play_string(str); + /* if it looks like a PLAY string... */ + if (strlen(argv1) > 2) { + if (*argv1 == 'M') { + play_string(argv1, false); + } } + } - errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); + errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); } diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d13ffe414..b286e0007 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -66,7 +66,7 @@ #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> @@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* control */ - /* if in auto mode, fly global position setpoint */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +#warning fix this +#if 0 + if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ || + vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) { /* simple heading control */ control_heading(&global_pos, &global_sp, &att, &att_sp); @@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* set flaps to zero */ actuators.control[4] = 0.0f; + } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) { /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { @@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } } } +#endif /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c96f73155..3125ce246 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,8 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_local_position.h> @@ -66,6 +67,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #include "flow_position_control_params.h" @@ -152,24 +154,34 @@ flow_position_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow position control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[fpc] started"); uint32_t counter = 0; const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct vehicle_status_s vstatus; + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_local_position_s local_pos; - + memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; + memset(&speed_sp, 0, sizeof(speed_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -213,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -249,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow position control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); } /* only run controller if position/speed changed */ @@ -258,7 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -267,13 +280,20 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); + + status_changed = true; + /* calc dt */ if(last_time == 0) { @@ -293,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[]) { flow_sp_sumy = filtered_flow.sumy; update_flow_sp_sumy = false; - } + } /* calc new bodyframe speed setpoints */ float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; @@ -490,7 +510,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* store actual height for speed estimation */ last_local_pos_z = local_pos.z; - speed_sp.thrust_sp = thrust_control; + speed_sp.thrust_sp = thrust_control; //manual.throttle; speed_sp.timestamp = hrt_absolute_time(); /* publish new speed setpoint */ @@ -515,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[]) else { /* in manual or stabilized state just reset speed and flow sum setpoint */ + //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); + + status_changed = false; speed_sp.vx = 0.0f; speed_sp.vy = 0.0f; flow_sp_sumx = filtered_flow.sumx; @@ -527,7 +552,6 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } - /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); @@ -556,27 +580,28 @@ flow_position_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow position control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow position control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); } } } } - printf("[flow position control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); thread_running = false; close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(vehicle_status_sub); + close(armed_sub); + close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index e40c9081d..495c415f2 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,8 @@ #include <math.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_local_position.h> @@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) printf("[flow position estimator] starting\n"); /* rotation matrix for transformation of optical flow speed vectors */ - static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, - { -1, 0, 0 }, + static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, + { 1, 0, 0 }, { 0, 0, 1 }}; // 90deg rotated const float time_scale = powf(10.0f,-6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; @@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to vehicle status */ - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to armed topic */ + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* subscribe to safety topic */ + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to attitude */ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -218,6 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { + if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -263,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ float sonar_new = flow.ground_distance_m; @@ -273,14 +281,15 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) * -> minimum sonar value 0.3m */ + if (!vehicle_liftoff) { - if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -336,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.y = local_pos.y + global_speed[1] * dt; local_pos.vx = global_speed[0]; local_pos.vy = global_speed[1]; + local_pos.xy_valid = true; + local_pos.v_xy_valid = true; } else { @@ -344,14 +355,17 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) filtered_flow.vy = 0; local_pos.vx = 0; local_pos.vy = 0; + local_pos.xy_valid = false; + local_pos.v_xy_valid = false; } /* filtering ground distance */ - if (!vehicle_liftoff || !vstatus.flag_system_armed) + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; + local_pos.z_valid = false; } else { @@ -379,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { local_pos.z = -sonar_new; } + + local_pos.z_valid = true; } filtered_flow.timestamp = hrt_absolute_time(); @@ -444,7 +460,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(vehicle_status_sub); + close(armed_sub); + close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 8b3881c43..feed0d23c 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,8 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> @@ -64,6 +65,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #include "flow_speed_control_params.h" @@ -150,21 +152,29 @@ flow_speed_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow speed control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd,"[fsc] started"); uint32_t counter = 0; /* structures */ - struct vehicle_status_s vstatus; + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; - + memset(&speed_sp, 0, sizeof(speed_sp)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -183,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -218,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow speed control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); } /* only run controller if position/speed changed */ @@ -226,19 +237,28 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of the armed topic */ + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + /* get a local copy of the control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { /* calc new roll/pitch */ float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); + + status_changed = true; + /* limit roll and pitch corrections */ if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) { @@ -294,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) } else { + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); + + status_changed = false; + /* reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -329,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow speed control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow speed control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fsp] initialized."); } } } } - printf("[flow speed control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fsc] ending now..."); thread_running = false; @@ -350,7 +375,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(vehicle_status_sub); + close(armed_sub); + close(control_mode_sub); close(att_sp_pub); perf_print_counter(mc_loop_perf); diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c new file mode 100644 index 000000000..06337be32 --- /dev/null +++ b/src/examples/hwtest/hwtest.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 hwtest.c + * + * Simple functional hardware test. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/actuator_controls.h> + +__EXPORT int ex_hwtest_main(int argc, char *argv[]); + +int ex_hwtest_main(int argc, char *argv[]) +{ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + int i; + float rcvalue = -1.0f; + hrt_abstime stime; + + while (true) { + stime = hrt_absolute_time(); + while (hrt_absolute_time() - stime < 1000000) { + for (i=0; i<8; i++) + actuators.control[i] = rcvalue; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + } + warnx("servos set to %.1f", rcvalue); + rcvalue *= -1.0f; + } + + return OK; +} diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk new file mode 100644 index 000000000..6e70863ed --- /dev/null +++ b/src/examples/hwtest/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Hardware test example application +# + +MODULE_COMMAND = ex_hwtest + +SRCS = hwtest.c diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 7f655964d..44e6dc7f3 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[]) int error_counter = 0; - while (true) { + for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index a28ff3a68..5054937e0 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -107,7 +107,6 @@ struct mavlink_logmessage { struct mavlink_logbuffer { unsigned int start; - // unsigned int end; unsigned int size; int count; struct mavlink_logmessage *elems; @@ -115,6 +114,8 @@ struct mavlink_logbuffer { void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); +void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); + int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 000000000..f5f59a2dc --- /dev/null +++ b/src/lib/conversion/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. +# +############################################################################ + +# +# Conversion library +# + +SRCS = rotation.cpp diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 000000000..b078562c2 --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 rotation.cpp + * + * Vector rotation library + */ + +#include "math.h" +#include "rotation.h" + +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3, 3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + (*rot_matrix)(i, j) = R(i, j); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 000000000..85c63c0fc --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 rotation.h + * + * Vector rotation library + */ + +#ifndef ROTATION_H_ +#define ROTATION_H_ + +#include <unistd.h> +#include <mathlib/mathlib.h> + +/** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct { + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = { + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** + * Get the rotation matrix + */ +__EXPORT void +get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + +#endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp new file mode 100644 index 000000000..2eb58abd6 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp + * Implementation of a simple orthogonal pitch PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_pitch_controller.h" +#include <math.h> +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_PitchController::ECL_PitchController() : + _last_run(0), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) +{ +} + +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, + bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* flying inverted (wings upside down) ? */ + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (roll > 0.0f) { + /* right hemisphere */ + roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); + } else { + /* left hemisphere */ + roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + + /* calculate the offset in the rate resulting from rolling */ + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll)) * _roll_ff; + if (inverted) + turn_offset = -turn_offset; + + float pitch_error = pitch_setpoint - pitch; + /* rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* add turn offset */ + _rate_setpoint += turn_offset; + + _rate_error = _rate_setpoint - pitch_rate; + + float ilimit_scaled = _integrator_max * scaler; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); +} + +void ECL_PitchController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h new file mode 100644 index 000000000..1e6cec6a1 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h + * Definition of a simple orthogonal pitch PID controller. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_PITCH_CONTROLLER_H +#define ECL_PITCH_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_PitchController +{ +public: + ECL_PitchController(); + + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + _tc = time_constant; + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate_pos(float max_rate_pos) { + _max_rate_pos = max_rate_pos; + } + void set_max_rate_neg(float max_rate_neg) { + _max_rate_neg = max_rate_neg; + } + void set_roll_ff(float roll_ff) { + _roll_ff = roll_ff; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _rate_setpoint; + } + +private: + + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate_pos; + float _max_rate_neg; + float _roll_ff; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _max_deflection_rad; +}; + +#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp new file mode 100644 index 000000000..0b1ffa7a4 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp + * Implementation of a simple orthogonal roll PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "../ecl.h" +#include "ecl_roll_controller.h" +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_RollController::ECL_RollController() : + _last_run(0), + _tc(0.1f), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) +{ + +} + +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + float dt = (float)dt_micros * 1e-6f; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + float roll_error = roll_setpoint - roll; + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + _rate_error = _rate_setpoint - roll_rate; + + + float ilimit_scaled = _integrator_max * scaler; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); +} + +void ECL_RollController::reset_integrator() +{ + _integrator = 0.0f; +} + diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h new file mode 100644 index 000000000..0d4ea9333 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h + * Definition of a simple orthogonal roll PID controller. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_ROLL_CONTROLLER_H +#define ECL_ROLL_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_RollController +{ +public: + ECL_RollController(); + + float control(float roll_setpoint, float roll, float roll_rate, + float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _rate_setpoint; + } + +private: + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _max_deflection_rad; +}; + +#endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp new file mode 100644 index 000000000..b786acf24 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_yaw_controller.h" +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_YawController::ECL_YawController() : + _last_run(0), + _last_error(0.0f), + _last_output(0.0f), + _last_rate_hp_out(0.0f), + _last_rate_hp_in(0.0f), + _k_d_last(0.0f), + _integrator(0.0f) +{ + +} + +float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, + float airspeed_min, float airspeed_max, float aspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + + return 0.0f; +} + +void ECL_YawController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h new file mode 100644 index 000000000..66b227918 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + */ +#ifndef ECL_YAW_CONTROLLER_H +#define ECL_YAW_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_YawController +{ +public: + ECL_YawController(); + + float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, + float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_k_side(float k_a) { + _k_side = k_a; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_k_roll_ff(float k_roll_ff) { + _k_roll_ff = k_roll_ff; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + +private: + uint64_t _last_run; + + float _k_side; + float _k_i; + float _k_d; + float _k_roll_ff; + float _integrator_max; + + float _last_error; + float _last_output; + float _last_rate_hp_out; + float _last_rate_hp_in; + float _k_d_last; + float _integrator; + +}; + +#endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h new file mode 100644 index 000000000..e0f207696 --- /dev/null +++ b/src/lib/ecl/ecl.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h + * Adapter / shim layer for system calls needed by ECL + * + */ + +#include <drivers/drv_hrt.h> +#include <geo/geo.h> + +#define ecl_absolute_time hrt_absolute_time +#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp new file mode 100644 index 000000000..27d76f959 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -0,0 +1,366 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include "ecl_l1_pos_controller.h" + +float ECL_L1_Pos_Controller::nav_roll() +{ + float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); + ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); + return ret; +} + +float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() +{ + return _lateral_accel; +} + +float ECL_L1_Pos_Controller::nav_bearing() +{ + return _wrap_pi(_nav_bearing); +} + +float ECL_L1_Pos_Controller::bearing_error() +{ + return _bearing_error; +} + +float ECL_L1_Pos_Controller::target_bearing() +{ + return _target_bearing; +} + +float ECL_L1_Pos_Controller::switch_distance(float wp_radius) +{ + /* following [2], switching on L1 distance */ + return math::max(wp_radius, _L1_distance); +} + +bool ECL_L1_Pos_Controller::reached_loiter_target(void) +{ + return _circle_mode; +} + +float ECL_L1_Pos_Controller::crosstrack_error(void) +{ + return _crosstrack_error; +} + +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed_vector) +{ + + /* this follows the logic presented in [1] */ + + float eta; + float xtrack_vel; + float ltrack_vel; + + /* get the direction between the last (visited) and next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + + /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate vector from A to B */ + math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + + /* + * check if waypoints are on top of each other. If yes, + * skip A and directly continue to B + */ + if (vector_AB.length() < 1.0e-6f) { + vector_AB = get_local_planar_vector(vector_curr_position, vector_B); + } + + vector_AB.normalize(); + + /* calculate the vector from waypoint A to the aircraft */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* calculate crosstrack error (output only) */ + _crosstrack_error = vector_AB % vector_A_to_airplane; + + /* + * If the current position is in a +-135 degree angle behind waypoint A + * and further away from A than the L1 distance, then A becomes the L1 point. + * If the aircraft is already between A and B normal L1 logic is applied. + */ + float distance_A_to_airplane = vector_A_to_airplane.length(); + float alongTrackDist = vector_A_to_airplane * vector_AB; + + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); + + /* extension from [2], fly directly to A */ + if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { + + /* calculate eta to fly to waypoint A */ + + /* unit vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ + + /* calculate eta to fly to waypoint B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + + } else { + + /* calculate eta to fly along the line between A and B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ + ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ + float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ + float xtrackErr = vector_A_to_airplane % vector_AB; + float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); + /* limit output to 45 degrees */ + sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 + float eta1 = asinf(sine_eta1); + eta = eta1 + eta2; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + + } + + /* limit angle to +-90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* flying to waypoints, not circling them */ + _circle_mode = false; + + /* the bearing angle, in NED frame */ + _bearing_error = eta; +} + +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + /* calculate the gains for the PD loop (circle tracking) */ + float omega = (2.0f * M_PI_F / _L1_period); + float K_crosstrack = omega * omega; + float K_velocity = 2.0f * _L1_damping * omega; + + /* update bearing to next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + + /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length() , 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate the vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* store the normalized vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + + /* calculate eta angle towards the loiter center */ + + /* velocity across / orthogonal to line from waypoint to current position */ + float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; + /* velocity along line from waypoint to current position */ + float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); + float eta = atan2f(xtrack_vel_center, ltrack_vel_center); + /* limit eta to 90 degrees */ + eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); + + /* calculate the lateral acceleration to capture the center point */ + float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* for PD control: Calculate radial position and velocity errors */ + + /* radial velocity error */ + float xtrack_vel_circle = -ltrack_vel_center; + /* radial distance from the loiter circle (not center) */ + float xtrack_err_circle = vector_A_to_airplane.length() - radius; + + /* cross track error for feedback */ + _crosstrack_error = xtrack_err_circle; + + /* calculate PD update to circle waypoint */ + float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); + + /* calculate velocity on circle / along tangent */ + float tangent_vel = xtrack_vel_center * loiter_direction; + + /* prevent PD output from turning the wrong way */ + if (tangent_vel < 0.0f) { + lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); + } + + /* calculate centripetal acceleration setpoint */ + float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); + + /* add PD control on circle and centripetal acceleration for total circle command */ + float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); + + /* + * Switch between circle (loiter) and capture (towards waypoint center) mode when + * the commands switch over. Only fly towards waypoint if outside the circle. + */ + + // XXX check switch over + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { + _lateral_accel = lateral_accel_sp_center; + _circle_mode = false; + /* angle between requested and current velocity vector */ + _bearing_error = eta; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + _lateral_accel = lateral_accel_sp_circle; + _circle_mode = true; + _bearing_error = 0.0f; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } +} + + +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + float eta; + + /* + * As the commanded heading is the only reference + * (and no crosstrack correction occurs), + * target and navigation bearing become the same + */ + _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); + eta = _target_bearing - _wrap_pi(current_heading); + eta = _wrap_pi(eta); + + /* consequently the bearing error is exactly eta: */ + _bearing_error = eta; + + /* ground speed is the length of the ground speed vector */ + float ground_speed = ground_speed_vector.length(); + + /* adjust L1 distance to keep constant frequency */ + _L1_distance = ground_speed / _heading_omega; + float omega_vel = ground_speed * _heading_omega; + + /* not circling a waypoint */ + _circle_mode = false; + + /* navigating heading means by definition no crosstrack error */ + _crosstrack_error = 0; + + /* limit eta to 90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = 2.0f * sinf(eta) * omega_vel; +} + +void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) +{ + /* the logic in this section is trivial, but originally proposed by [2] */ + + /* reset all heading / error measures resulting in zero roll */ + _target_bearing = current_heading; + _nav_bearing = current_heading; + _bearing_error = 0; + _crosstrack_error = 0; + _lateral_accel = 0; + + /* not circling a waypoint when flying level */ + _circle_mode = false; + +} + + +math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +{ + /* this is an approximation for small angles, proposed by [2] */ + + math::Vector2f out; + + out.setX(math::radians((target.getX() - origin.getX()))); + out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + + return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH); +} + diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h new file mode 100644 index 000000000..7a3c42a92 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h + * Implementation of L1 position control. + * + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the original + * publication from [1] and does include a lot of the ideas (not code) + * from [2]. + * + * + * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle + * + */ + +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H + +#include <mathlib/mathlib.h> +#include <geo/geo.h> +#include <ecl/ecl.h> + +/** + * L1 Nonlinear Guidance Logic + */ +class __EXPORT ECL_L1_Pos_Controller +{ +public: + ECL_L1_Pos_Controller() { + _L1_period = 25; + _L1_damping = 0.75f; + } + + /** + * The current target bearing + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float nav_bearing(); + + + /** + * Get lateral acceleration demand. + * + * @return Lateral acceleration in m/s^2 + */ + float nav_lateral_acceleration_demand(); + + + /** + * Heading error. + * + * The heading error is either compared to the current track + * or to the tangent of the current loiter radius. + */ + float bearing_error(); + + + /** + * Bearing from aircraft to current target. + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float target_bearing(); + + + /** + * Get roll angle setpoint for fixed wing. + * + * @return Roll angle (in NED frame) + */ + float nav_roll(); + + + /** + * Get the current crosstrack error. + * + * @return Crosstrack error in meters. + */ + float crosstrack_error(); + + + /** + * Returns true if the loiter waypoint has been reached + */ + bool reached_loiter_target(); + + + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + + /** + * Get the switch distance + * + * This is the distance at which the system will + * switch to the next waypoint. This depends on the + * period and damping + * + * @param waypoint_switch_radius The switching radius the waypoint has set. + */ + float switch_distance(float waypoint_switch_radius); + + + /** + * Navigate between two waypoints + * + * Calling this function with two waypoints results in the + * control outputs to fly to the line segment defined by + * the points and once captured following the line segment. + * This follows the logic in [1]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed); + + + /** + * Navigate on an orbit around a loiter waypoint. + * + * This allow orbits smaller than the L1 length, + * this modification was introduced in [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector); + + + /** + * Navigate on a fixed bearing. + * + * This only holds a certain direction and does not perform cross + * track correction. Helpful for semi-autonomous modes. Introduced + * by [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + + + /** + * Keep the wings level. + * + * This is typically needed for maximum-lift-demand situations, + * such as takeoff or near stall. Introduced in [2]. + */ + void navigate_level_flight(float current_heading); + + + /** + * Set the L1 period. + */ + void set_l1_period(float period) { + _L1_period = period; + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate normalized frequency for heading tracking */ + _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; + } + + + /** + * Set the L1 damping factor. + * + * The original publication recommends a default of sqrt(2) / 2 = 0.707 + */ + void set_l1_damping(float damping) { + _L1_damping = damping; + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate the L1 gain (following [2]) */ + _K_L1 = 4.0f * _L1_damping * _L1_damping; + } + + + /** + * Set the maximum roll angle output in radians + * + */ + void set_l1_roll_limit(float roll_lim_rad) { + _roll_lim_rad = roll_lim_rad; + } + +private: + + float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 + float _L1_distance; ///< L1 lead distance, defined by period and damping + bool _circle_mode; ///< flag for loiter mode + float _nav_bearing; ///< bearing to L1 reference point + float _bearing_error; ///< bearing error + float _crosstrack_error; ///< crosstrack error in meters + float _target_bearing; ///< the heading setpoint + + float _L1_period; ///< L1 tracking period in seconds + float _L1_damping; ///< L1 damping ratio + float _L1_ratio; ///< L1 ratio for navigation + float _K_L1; ///< L1 control gain for _L1_damping + float _heading_omega; ///< Normalized frequency + + float _roll_lim_rad; ///<maximum roll angle + + /** + * Convert a 2D vector from WGS84 to planar coordinates. + * + * This converts from latitude and longitude to planar + * coordinates with (0,0) being at the position of ref and + * returns a vector in meters towards wp. + * + * @param ref The reference position in WGS84 coordinates + * @param wp The point to convert to into the local coordinates, in WGS84 coordinates + * @return The vector in meters pointing from the reference position to the coordinates + */ + math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + +}; + + +#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk new file mode 100644 index 000000000..f2aa3db6a --- /dev/null +++ b/src/lib/ecl/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013 Estimation and Control Library (ECL). 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. +# +############################################################################ + +# +# Estimation and Control Library +# + +SRCS = attitude_fw/ecl_pitch_controller.cpp \ + attitude_fw/ecl_roll_controller.cpp \ + attitude_fw/ecl_yaw_controller.cpp \ + l1/ecl_l1_pos_controller.cpp diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk new file mode 100644 index 000000000..53f1629e3 --- /dev/null +++ b/src/lib/external_lgpl/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# W A R N I N G: The contents of this directory are license-incompatible +# with the rest of the codebase. Do NOT copy any parts of it +# into other folders. +# +# Acknowledgements: +# +# The algorithms in this folder have been developed by Paul Riseborough +# with support from Andrew Tridgell. +# Originally licensed as LGPL for APM. As this is built as library and +# linked, use of this code does not change the usability of PX4 in general +# or any of the license implications. +# + +SRCS = tecs/tecs.cpp diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp new file mode 100644 index 000000000..864a9d24d --- /dev/null +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -0,0 +1,534 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "tecs.h" +#include <ecl/ecl.h> + +using namespace math; + +#ifndef CONSTANTS_ONE_G +#define CONSTANTS_ONE_G GRAVITY +#endif + +/** + * @file tecs.cpp + * + * @author Paul Riseborough + * + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle and switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use + * of easy to measure aircraft performance data + * + */ + +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +{ + // Implement third order complementary filter for height and height rate + // estimted height rate = _integ2_state + // estimated height = _integ3_state + // Reference Paper : + // Optimising the Gains of the Baro-Inertial Vertical Channel + // Widnall W.S, Sinha P.K, + // AIAA Journal of Guidance and Control, 78-1307R + + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f; + + // printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n", + // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), + // accel_earth(0), accel_earth(1), accel_earth(2)); + + if (DT > 1.0f) { + _integ3_state = baro_altitude; + _integ2_state = 0.0f; + _integ1_state = 0.0f; + DT = 0.02f; // when first starting TECS, use a + // small time constant + } + + _update_50hz_last_usec = now; + _EAS = airspeed; + + // Get height acceleration + float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); + // Perform filter calculation using backwards Euler integration + // Coefficients selected to place all three filter poles at omega + float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega; + float hgt_err = baro_altitude - _integ3_state; + float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; + _integ1_state = _integ1_state + integ1_input * DT; + float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; + _integ2_state = _integ2_state + integ2_input * DT; + float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f; + + // If more than 1 second has elapsed since last update then reset the integrator state + // to the measured height + if (DT > 1.0f) { + _integ3_state = baro_altitude; + + } else { + _integ3_state = _integ3_state + integ3_input * DT; + } + + // Update and average speed rate of change + // Only required if airspeed is being measured and controlled + float temp = 0; + + if (isfinite(airspeed) && airspeed_sensor_enabled()) { + // Get DCM + // Calculate speed rate of change + // XXX check + temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); + // take 5 point moving average + //_vel_dot = _vdot_filter.apply(temp); + // XXX resolve this properly + _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + + } else { + _vel_dot = 0.0f; + } + +} + +void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; + _update_speed_last_usec = now; + + // Convert equivalent airspeeds to true airspeeds + + _EAS_dem = airspeed_demand; + _TAS_dem = _EAS_dem * EAS2TAS; + _TASmax = indicated_airspeed_max * EAS2TAS; + _TASmin = indicated_airspeed_min * EAS2TAS; + + // Reset states of time since last update is too large + if (DT > 1.0f) { + _integ5_state = (_EAS * EAS2TAS); + _integ4_state = 0.0f; + DT = 0.1f; // when first starting TECS, use a + // small time constant + } + + // Get airspeed or default to halfway between min and max if + // airspeed is not being used and set speed rate to zero + if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + // If no airspeed available use average of min and max + _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); + + } else { + _EAS = indicated_airspeed; + } + + // Implement a second order complementary filter to obtain a + // smoothed airspeed estimate + // airspeed estimate is held in _integ5_state + float aspdErr = (_EAS * EAS2TAS) - _integ5_state; + float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; + + // Prevent state from winding up + if (_integ5_state < 3.1f) { + integ4_input = max(integ4_input , 0.0f); + } + + _integ4_state = _integ4_state + integ4_input * DT; + float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; + _integ5_state = _integ5_state + integ5_input * DT; + // limit the airspeed to a minimum of 3 m/s + _integ5_state = max(_integ5_state, 3.0f); + +} + +void TECS::_update_speed_demand(void) +{ + // Set the airspeed demand to the minimum value if an underspeed condition exists + // or a bad descent condition exists + // This will minimise the rate of descent resulting from an engine failure, + // enable the maximum climb rate to be achieved and prevent continued full power descent + // into the ground due to an unachievable airspeed value + if ((_badDescent) || (_underspeed)) { + _TAS_dem = _TASmin; + } + + // Constrain speed demand + _TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax); + + // calculate velocity rate limits based on physical performance limits + // provision to use a different rate limit if bad descent or underspeed condition exists + // Use 50% of maximum energy rate to allow margin for total energy contgroller + float velRateMax; + float velRateMin; + + if ((_badDescent) || (_underspeed)) { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + + } else { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + } + + // Apply rate limit + if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; + _TAS_rate_dem = velRateMax; + + } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; + _TAS_rate_dem = velRateMin; + + } else { + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; + } + + // Constrain speed demand again to protect against bad values on initialisation. + _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); + _TAS_dem_last = _TAS_dem; +} + +void TECS::_update_height_demand(float demand) +{ + // Apply 2 point moving average to demanded height + // This is required because height demand is only updated at 5Hz + _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); + _hgt_dem_in_old = _hgt_dem; + + // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, + // _maxClimbRate); + + // Limit height rate of change + if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + + } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } + + _hgt_dem_prev = _hgt_dem; + + // Apply first order lag to height demand + _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; + _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; + _hgt_dem_adj_last = _hgt_dem_adj; + + // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, + // _hgt_rate_dem); +} + +void TECS::_detect_underspeed(void) +{ + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { + _underspeed = true; + + } else { + _underspeed = false; + } +} + +void TECS::_update_energies(void) +{ + // Calculate specific energy demands + _SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G; + _SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj; + + // Calculate specific energy rate demands + _SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G; + _SKEdot_dem = _integ5_state * _TAS_rate_dem; + + // Calculate specific energy + _SPE_est = _integ3_state * CONSTANTS_ONE_G; + _SKE_est = 0.5f * _integ5_state * _integ5_state; + + // Calculate specific energy rate + _SPEdot = _integ2_state * CONSTANTS_ONE_G; + _SKEdot = _integ5_state * _vel_dot; +} + +void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +{ + // Calculate total energy values + _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; + float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); + float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + + // Apply 0.5 second first order filter to STEdot_error + // This is required to remove accelerometer noise from the measurement + STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = STEdot_error; + + // Calculate throttle demand + // If underspeed condition is set, then demand full throttle + if (_underspeed) { + _throttle_dem_unc = 1.0f; + + } else { + // Calculate gain scaler from specific energy error to throttle + float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + + // Calculate feed-forward throttle + float ff_throttle = 0; + float nomThr = throttle_cruise; + // Use the demanded rate of change of total energy as the feed-forward demand, but add + // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // drag increase during turns. + float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + + if (STEdot_dem >= 0) { + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + + } else { + ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; + } + + // Calculate PD + FF throttle + _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + + // Rate limit PD + FF throttle + // Calculate the throttle increment from the specified slew time + if (fabsf(_throttle_slewrate) < 0.01f) { + float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; + + _throttle_dem = constrain(_throttle_dem, + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); + _last_throttle_dem = _throttle_dem; + } + + + // Calculate integrator state upper and lower limits + // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + float integ_max = (_THRmaxf - _throttle_dem + 0.1f); + float integ_min = (_THRminf - _throttle_dem - 0.1f); + + // Calculate integrator state, constraining state + // Set integrator to a max throttle value dduring climbout + _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; + + if (_climbOutDem) { + _integ6_state = integ_max; + + } else { + _integ6_state = constrain(_integ6_state, integ_min, integ_max); + } + + // Sum the components. + // Only use feed-forward component if airspeed is not being used + if (airspeed_sensor_enabled()) { + _throttle_dem = _throttle_dem + _integ6_state; + + } else { + _throttle_dem = ff_throttle; + } + } + + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); +} + +void TECS::_detect_bad_descent(void) +{ + // Detect a demanded airspeed too high for the aircraft to achieve. This will be + // evident by the the following conditions: + // 1) Underspeed protection not active + // 2) Specific total energy error > 200 (greater than ~20m height error) + // 3) Specific total energy reducing + // 4) throttle demand > 90% + // If these four conditions exist simultaneously, then the protection + // mode will be activated. + // Once active, the following condition are required to stay in the mode + // 1) Underspeed protection not active + // 2) Specific total energy error > 0 + // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set + float STEdot = _SPEdot + _SKEdot; + + if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { + _badDescent = true; + + } else { + _badDescent = false; + } +} + +void TECS::_update_pitch(void) +{ + // Calculate Speed/Height Control Weighting + // This is used to determine how the pitch control prioritises speed and height control + // A weighting of 1 provides equal priority (this is the normal mode of operation) + // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available + // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected + // or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed + // rises above the demanded value, the pitch angle will be increased by the TECS controller. + float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f); + + if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) { + SKE_weighting = 2.0f; + + } else if (!airspeed_sensor_enabled()) { + SKE_weighting = 0.0f; + } + + float SPE_weighting = 2.0f - SKE_weighting; + + // Calculate Specific Energy Balance demand, and error + float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; + float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; + float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + + // Calculate integrator state, constraining input if pitch limits are exceeded + float integ7_input = SEB_error * _integGain; + + if (_pitch_dem_unc > _PITCHmaxf) { + integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); + + } else if (_pitch_dem_unc < _PITCHminf) { + integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc); + } + + _integ7_state = _integ7_state + integ7_input * _DT; + + // Apply max and min values for integrator state that will allow for no more than + // 5deg of saturation. This allows for some pitch variation due to gusts before the + // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); + float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + + // Calculate pitch demand from specific energy balance signals + _pitch_dem_unc = (temp + _integ7_state) / gainInv; + + // Constrain pitch demand + _pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); + + // Rate limit the pitch demand to comply with specified vertical + // acceleration limit + float ptchRateIncr = _DT * _vertAccLim / _integ5_state; + + if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { + _pitch_dem = _last_pitch_dem + ptchRateIncr; + + } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { + _pitch_dem = _last_pitch_dem - ptchRateIncr; + } + + _last_pitch_dem = _pitch_dem; +} + +void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) +{ + // Initialise states and variables if DT > 1 second or in climbout + if (_DT > 1.0f) { + _integ6_state = 0.0f; + _integ7_state = 0.0f; + _last_throttle_dem = throttle_cruise; + _last_pitch_dem = pitch; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _hgt_dem_in_old = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + _DT = 0.1f; // when first starting TECS, use a + // small time constant + + } else if (_climbOutDem) { + _PITCHminf = ptchMinCO_rad; + _THRminf = _THRmaxf - 0.01f; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + } +} + +void TECS::_update_STE_rate_lim(void) +{ + // Calculate Specific Total Energy Rate Limits + // This is a tivial calculation at the moment but will get bigger once we start adding altitude effects + _STEdot_max = _maxClimbRate * CONSTANTS_ONE_G; + _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; +} + +void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; + _update_pitch_throttle_last_usec = now; + + // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", + // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); + + // Update the speed estimate using a 2nd order complementary filter + _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); + + // Convert inputs + _THRmaxf = throttle_max; + _THRminf = throttle_min; + _PITCHmaxf = pitch_limit_max; + _PITCHminf = pitch_limit_min; + _climbOutDem = climbOutDem; + + // initialise selected states and variables if DT > 1 second or in climbout + _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + + // Calculate Specific Total Energy Rate Limits + _update_STE_rate_lim(); + + // Calculate the speed demand + _update_speed_demand(); + + // Calculate the height demand + _update_height_demand(hgt_dem); + + // Detect underspeed condition + _detect_underspeed(); + + // Calculate specific energy quantitiues + _update_energies(); + + // Calculate throttle demand + _update_throttle(throttle_cruise, rotMat); + + // Detect bad descent due to demanded airspeed being too high + _detect_bad_descent(); + + // Calculate pitch demand + _update_pitch(); + +// // Write internal variables to the log_tuning structure. This +// // structure will be logged in dataflash at 10Hz + // log_tuning.hgt_dem = _hgt_dem_adj; + // log_tuning.hgt = _integ3_state; + // log_tuning.dhgt_dem = _hgt_rate_dem; + // log_tuning.dhgt = _integ2_state; + // log_tuning.spd_dem = _TAS_dem_adj; + // log_tuning.spd = _integ5_state; + // log_tuning.dspd = _vel_dot; + // log_tuning.ithr = _integ6_state; + // log_tuning.iptch = _integ7_state; + // log_tuning.thr = _throttle_dem; + // log_tuning.ptch = _pitch_dem; + // log_tuning.dspd_dem = _TAS_rate_dem; +} diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h new file mode 100644 index 000000000..f8f832ed7 --- /dev/null +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -0,0 +1,357 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file tecs.h +/// @brief Combined Total Energy Speed & Height Control. + +/* + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use + * of easy to measure aircraft performance data + */ + +#ifndef TECS_H +#define TECS_H + +#include <mathlib/mathlib.h> +#include <stdint.h> + +class __EXPORT TECS +{ +public: + TECS() : + + _airspeed_enabled(false), + _throttle_slewrate(0.0f), + _climbOutDem(false), + _hgt_dem_prev(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_in_old(0.0f), + _TAS_dem_last(0.0f), + _TAS_dem_adj(0.0f), + _TAS_dem(0.0f), + _integ1_state(0.0f), + _integ2_state(0.0f), + _integ3_state(0.0f), + _integ4_state(0.0f), + _integ5_state(0.0f), + _integ6_state(0.0f), + _integ7_state(0.0f), + _pitch_dem(0.0f), + _last_pitch_dem(0.0f), + _SPE_dem(0.0f), + _SKE_dem(0.0f), + _SPEdot_dem(0.0f), + _SKEdot_dem(0.0f), + _SPE_est(0.0f), + _SKE_est(0.0f), + _SPEdot(0.0f), + _SKEdot(0.0f), + _vel_dot(0.0f), + _STEdotErrLast(0.0f) { + + } + + bool airspeed_sensor_enabled() { + return _airspeed_enabled; + } + + void enable_airspeed(bool enabled) { + _airspeed_enabled = enabled; + } + + // Update of the estimated height and height rate internal state + // Update of the inertial speed rate internal state + // Should be called at 50Hz or greater + void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + + // Update the control loop calculations + void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max); + // demanded throttle in percentage + // should return 0 to 100 + float get_throttle_demand(void) { + return _throttle_dem; + } + int32_t get_throttle_demand_percent(void) { + return get_throttle_demand(); + } + + + float get_pitch_demand() { return _pitch_dem; } + + // demanded pitch angle in centi-degrees + // should return between -9000 to +9000 + int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);} + + // Rate of change of velocity along X body axis in m/s^2 + float get_VXdot(void) { return _vel_dot; } + + + float get_speed_weight() { + return _spdWeight; + } + + // log data on internal state of the controller. Called at 10Hz + // void log_data(DataFlash_Class &dataflash, uint8_t msgid); + + // struct PACKED log_TECS_Tuning { + // LOG_PACKET_HEADER; + // float hgt; + // float dhgt; + // float hgt_dem; + // float dhgt_dem; + // float spd_dem; + // float spd; + // float dspd; + // float ithr; + // float iptch; + // float thr; + // float ptch; + // float dspd_dem; + // } log_tuning; + + void set_time_const(float time_const) { + _timeConst = time_const; + } + + void set_min_sink_rate(float rate) { + _minSinkRate = rate; + } + + void set_max_sink_rate(float sink_rate) { + _maxSinkRate = sink_rate; + } + + void set_max_climb_rate(float climb_rate) { + _maxClimbRate = climb_rate; + } + + void set_throttle_damp(float throttle_damp) { + _thrDamp = throttle_damp; + } + + void set_integrator_gain(float gain) { + _integGain = gain; + } + + void set_vertical_accel_limit(float limit) { + _vertAccLim = limit; + } + + void set_height_comp_filter_omega(float omega) { + _hgtCompFiltOmega = omega; + } + + void set_speed_comp_filter_omega(float omega) { + _spdCompFiltOmega = omega; + } + + void set_roll_throttle_compensation(float compensation) { + _rollComp = compensation; + } + + void set_speed_weight(float weight) { + _spdWeight = weight; + } + + void set_pitch_damping(float damping) { + _ptchDamp = damping; + } + + void set_throttle_slewrate(float slewrate) { + _throttle_slewrate = slewrate; + } + + void set_indicated_airspeed_min(float airspeed) { + _indicated_airspeed_min = airspeed; + } + + void set_indicated_airspeed_max(float airspeed) { + _indicated_airspeed_max = airspeed; + } + +private: + // Last time update_50Hz was called + uint64_t _update_50hz_last_usec; + + // Last time update_speed was called + uint64_t _update_speed_last_usec; + + // Last time update_pitch_throttle was called + uint64_t _update_pitch_throttle_last_usec; + + // TECS tuning parameters + float _hgtCompFiltOmega; + float _spdCompFiltOmega; + float _maxClimbRate; + float _minSinkRate; + float _maxSinkRate; + float _timeConst; + float _ptchDamp; + float _thrDamp; + float _integGain; + float _vertAccLim; + float _rollComp; + float _spdWeight; + + // throttle demand in the range from 0.0 to 1.0 + float _throttle_dem; + + // pitch angle demand in radians + float _pitch_dem; + + // Integrator state 1 - height filter second derivative + float _integ1_state; + + // Integrator state 2 - height rate + float _integ2_state; + + // Integrator state 3 - height + float _integ3_state; + + // Integrator state 4 - airspeed filter first derivative + float _integ4_state; + + // Integrator state 5 - true airspeed + float _integ5_state; + + // Integrator state 6 - throttle integrator + float _integ6_state; + + // Integrator state 6 - pitch integrator + float _integ7_state; + + // throttle demand rate limiter state + float _last_throttle_dem; + + // pitch demand rate limiter state + float _last_pitch_dem; + + // Rate of change of speed along X axis + float _vel_dot; + + // Equivalent airspeed + float _EAS; + + // True airspeed limits + float _TASmax; + float _TASmin; + + // Current and last true airspeed demand + float _TAS_dem; + float _TAS_dem_last; + + // Equivalent airspeed demand + float _EAS_dem; + + // height demands + float _hgt_dem; + float _hgt_dem_in_old; + float _hgt_dem_adj; + float _hgt_dem_adj_last; + float _hgt_rate_dem; + float _hgt_dem_prev; + + // Speed demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_dem_adj; + + // Speed rate demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_rate_dem; + + // Total energy rate filter state + float _STEdotErrLast; + + // Underspeed condition + bool _underspeed; + + // Bad descent condition caused by unachievable airspeed demand + bool _badDescent; + + // climbout mode + bool _climbOutDem; + + // throttle demand before limiting + float _throttle_dem_unc; + + // pitch demand before limiting + float _pitch_dem_unc; + + // Maximum and minimum specific total energy rate limits + float _STEdot_max; + float _STEdot_min; + + // Maximum and minimum floating point throttle limits + float _THRmaxf; + float _THRminf; + + // Maximum and minimum floating point pitch limits + float _PITCHmaxf; + float _PITCHminf; + + // Specific energy quantities + float _SPE_dem; + float _SKE_dem; + float _SPEdot_dem; + float _SKEdot_dem; + float _SPE_est; + float _SKE_est; + float _SPEdot; + float _SKEdot; + + // Specific energy error quantities + float _STE_error; + + // Time since last update of main TECS loop (seconds) + float _DT; + + bool _airspeed_enabled; + float _throttle_slewrate; + float _indicated_airspeed_min; + float _indicated_airspeed_max; + + // Update the airspeed internal state using a second order complementary filter + void _update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS); + + // Update the demanded airspeed + void _update_speed_demand(void); + + // Update the demanded height + void _update_height_demand(float demand); + + // Detect an underspeed condition + void _detect_underspeed(void); + + // Update Specific Energy Quantities + void _update_energies(void); + + // Update Demanded Throttle + void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + + // Detect Bad Descent + void _detect_bad_descent(void); + + // Update Demanded Pitch Angle + void _update_pitch(void); + + // Initialise states and variables + void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad); + + // Calculate specific total energy rate limits + void _update_STE_rate_lim(void); + +}; + +#endif //TECS_H diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c index 6463e6489..43105fdba 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -44,7 +44,7 @@ * @author Lorenz Meier <lm@inf.ethz.ch> */ -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <nuttx/config.h> #include <unistd.h> #include <pthread.h> @@ -210,6 +210,36 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); +} + +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); +} + // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h index dadec51ec..123ff80f1 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -57,10 +57,6 @@ __BEGIN_DECLS #define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ #define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - // XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment @@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk new file mode 100644 index 000000000..30a2dc99f --- /dev/null +++ b/src/lib/geo/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Geo library +# + +SRCS = geo.c diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h index 8f39acd9d..8f39acd9d 100644 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h index 181b7e433..181b7e433 100644 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h index 9c37ab4e5..9c37ab4e5 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h +++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h index 406f737dc..406f737dc 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h +++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h index 6f66f9ee3..6f66f9ee3 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_math.h +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h index 8ac6dc078..8ac6dc078 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm3.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h index 93efd3a7a..93efd3a7a 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm4.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h index af1831ee1..af1831ee1 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h index 139bc3c5e..139bc3c5e 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h +++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h index 8946c2c49..8946c2c49 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h +++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a Binary files differindex 6898bc27d..6898bc27d 100644 --- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a Binary files differindex a0185eaa9..a0185eaa9 100755 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a Binary files differindex 94525528e..94525528e 100755 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk index 0cc1b559d..0cc1b559d 100644 --- a/src/modules/mathlib/CMSIS/library.mk +++ b/src/lib/mathlib/CMSIS/library.mk diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt index 31afac1ec..31afac1ec 100644 --- a/src/modules/mathlib/CMSIS/license.txt +++ b/src/lib/mathlib/CMSIS/license.txt diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp index 5c15d4d6d..5c15d4d6d 100644 --- a/src/modules/mathlib/math/Dcm.cpp +++ b/src/lib/mathlib/math/Dcm.cpp diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp index 38f697c15..38f697c15 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/lib/mathlib/math/Dcm.hpp diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp index e733d23bb..e733d23bb 100644 --- a/src/modules/mathlib/math/EulerAngles.cpp +++ b/src/lib/mathlib/math/EulerAngles.cpp diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp index 399eecfa7..399eecfa7 100644 --- a/src/modules/mathlib/math/EulerAngles.hpp +++ b/src/lib/mathlib/math/EulerAngles.hpp diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a..d4c892d8a 100644 --- a/src/modules/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd66..fb778dd66 100644 --- a/src/modules/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp index ebd1aeda3..ebd1aeda3 100644 --- a/src/modules/mathlib/math/Matrix.cpp +++ b/src/lib/mathlib/math/Matrix.cpp diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..f19db15ec 100644 --- a/src/modules/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp index 02fec4ca6..02fec4ca6 100644 --- a/src/modules/mathlib/math/Quaternion.cpp +++ b/src/lib/mathlib/math/Quaternion.cpp diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d33..048a55d33 100644 --- a/src/modules/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp index 35158a396..35158a396 100644 --- a/src/modules/mathlib/math/Vector.cpp +++ b/src/lib/mathlib/math/Vector.cpp diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..73de793d5 100644 --- a/src/modules/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp index 68e741817..68e741817 100644 --- a/src/modules/mathlib/math/Vector2f.cpp +++ b/src/lib/mathlib/math/Vector2f.cpp diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp index ecd62e81c..ecd62e81c 100644 --- a/src/modules/mathlib/math/Vector2f.hpp +++ b/src/lib/mathlib/math/Vector2f.hpp diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp index 3936650c6..3936650c6 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/lib/mathlib/math/Vector3.cpp diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 2bf00f26b..2bf00f26b 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp index 21661622a..21661622a 100644 --- a/src/modules/mathlib/math/arm/Matrix.cpp +++ b/src/lib/mathlib/math/arm/Matrix.cpp diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp index 715fd3a5e..1945bb02d 100644 --- a/src/modules/mathlib/math/arm/Matrix.hpp +++ b/src/lib/mathlib/math/arm/Matrix.hpp @@ -121,10 +121,10 @@ public: for (size_t i = 0; i < getRows(); i++) { for (size_t j = 0; j < getCols(); j++) { float sig; - int exp; + int exponent; float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp index 7ea6496bb..7ea6496bb 100644 --- a/src/modules/mathlib/math/arm/Vector.cpp +++ b/src/lib/mathlib/math/arm/Vector.cpp diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp index 0f7722143..a2526d0a2 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/lib/mathlib/math/arm/Vector.hpp @@ -109,10 +109,10 @@ public: inline void print() const { for (size_t i = 0; i < getRows(); i++) { float sig; - int exp; + int exponent; float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index efb17225d..efb17225d 100644 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4..208ec98d4 100644 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk index fe92c8c70..f5c0647c8 100644 --- a/src/modules/mathlib/math/filter/module.mk +++ b/src/lib/mathlib/math/filter/module.mk @@ -41,4 +41,3 @@ SRCS = LowPassFilter2p.cpp # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp index 21661622a..21661622a 100644 --- a/src/modules/mathlib/math/generic/Matrix.cpp +++ b/src/lib/mathlib/math/generic/Matrix.cpp diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp index 5601a3447..5601a3447 100644 --- a/src/modules/mathlib/math/generic/Matrix.hpp +++ b/src/lib/mathlib/math/generic/Matrix.hpp diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp index 7ea6496bb..7ea6496bb 100644 --- a/src/modules/mathlib/math/generic/Vector.cpp +++ b/src/lib/mathlib/math/generic/Vector.cpp diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp index 8cfdc676d..8cfdc676d 100644 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ b/src/lib/mathlib/math/generic/Vector.hpp diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf Binary files differindex eb67a4bfc..eb67a4bfc 100644 --- a/src/modules/mathlib/math/nasa_rotation_def.pdf +++ b/src/lib/mathlib/math/nasa_rotation_def.pdf diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 2fa2f7e7c..2fa2f7e7c 100644 --- a/src/modules/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 2027bb827..2027bb827 100644 --- a/src/modules/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce index c3fba4729..c3fba4729 100644 --- a/src/modules/mathlib/math/test_math.sce +++ b/src/lib/mathlib/math/test_math.sce diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc..40ffb22bc 100644 --- a/src/modules/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk index 2146a1413..72bc7db8a 100644 --- a/src/modules/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -49,7 +49,6 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) INCLUDE_DIRS += math/arm diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f30..18fb6dcbc 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -34,7 +34,7 @@ /** * @file KalmanNav.cpp * - * kalman filter navigation code + * Kalman filter navigation code */ #include <poll.h> @@ -228,10 +228,7 @@ void KalmanNav::update() updateSubscriptions(); // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate && - _sensors.accelerometer_counter > 10 && - _sensors.gyro_counter > 10 && - _sensors.magnetometer_counter > 10) { + if (!_attitudeInitialized && sensorsUpdate) { if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { @@ -325,7 +322,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; @@ -643,7 +640,7 @@ int KalmanNav::correctAtt() if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:\n"); y.print(); + warnx("y:"); y.print(); } // update quaternions from euler diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee0355..799fc2fb9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam<float> _vGyro; /**< gyro process noise */ - control::BlockParam<float> _vAccel; /**< accelerometer process noise */ - control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ - control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ - control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ - control::BlockParam<float> _magDip; /**< magnetic inclination with level */ - control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam<float> _g; /**< gravitational constant */ - control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ - control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; 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 9e533ccdf..a70a14fe4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> @@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // XXX disabling init for now + initialized = true; + + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; + + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { 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 107c2dfb1..86bda3c75 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 @@ -35,7 +35,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> @@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp index 6a304896a..5eeca5a1a 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file accelerometer_calibration.c + * @file accelerometer_calibration.cpp * * Implementation of accelerometer calibration. * @@ -100,158 +100,234 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin <anton.babushkin@me.com> */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" +#include "commander_helper.h" +#include <unistd.h> +#include <stdio.h> #include <poll.h> +#include <fcntl.h> +#include <sys/prctl.h> +#include <math.h> +#include <mathlib/mathlib.h> +#include <string.h> #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> -#include <systemlib/conversions.h> +#include <geo/geo.h> +#include <conversion/rotation.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> #include <mavlink/mavlink_log.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "accel"; + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); +int do_accel_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - /* measure and calculate offsets & scales */ float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + float accel_T[3][3]; if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + /* measure and calculate offsets & scales */ + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + } + + if (res == OK) { + /* measurements completed successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + math::Matrix board_rotation_t = board_rotation.transpose(); + math::Vector3 accel_offs_vec; + accel_offs_vec.set(&accel_offs[0]); + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix accel_T_mat(3, 3); + accel_T_mat.set(&accel_T[0][0]); + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); + + /* set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + res = ERROR; } + } + if (res == OK) { + /* apply new scaling and offsets */ int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (res == OK) { /* auto-save to EEPROM */ - int save_ret = param_save_default(); + res = param_save_default(); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_error(); - sleep(2); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + return res; } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) +{ const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } + unsigned done_count = 0; + int res = OK; - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; - char str[80]; - int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; + for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + if (data_collected[i]) { + done_count++; + + } else { done = false; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + if (done) break; - mavlink_log_info(mavlink_fd, str); + + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) - return ERROR; - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + if (orient < 0) { + res = ERROR; + break; + } + + if (data_collected[orient]) { + mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + continue; + } + + mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); + data_collected[orient] = true; - tune_confirm(); + tune_neutral(); } + close(sensor_combined_sub); - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); - return ERROR; - } + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -260,21 +336,24 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { +int detect_orientation(int mavlink_fd, int sub_sensor_combined) +{ struct sensor_combined_s sensor; /* exponential moving average of accel */ float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* 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 */ - int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + hrt_abstime still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ @@ -289,108 +368,137 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; + for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + if (d > accel_disp[i]) accel_disp[i] = d; } + /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still..."); + mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; + } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); + mavlink_log_info(mavlink_fd, "detected motion, hold still..."); t_still = 0; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); - return -1; + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return ERROR; } } - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) +{ + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; 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) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) 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++) { @@ -400,11 +508,13 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp return OK; } -int mat_invert3(float src[3][3], float dst[3][3]) { +int mat_invert3(float src[3][3], float dst[3][3]) +{ float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; @@ -420,7 +530,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) { return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +{ /* calculate offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; @@ -429,6 +540,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { float a = accel_ref[i * 2][j] - accel_offs[j]; @@ -438,6 +550,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) return ERROR; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index f93a867ba..1cf9c0977 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -44,8 +44,7 @@ #define ACCELEROMETER_CALIBRATION_H_ #include <stdint.h> -#include <uORB/topics/vehicle_status.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp new file mode 100644 index 000000000..248eb4a66 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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_calibration.cpp + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include <stdio.h> +#include <poll.h> +#include <math.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/differential_pressure.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + close(diff_pres_sub); + return ERROR; + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + close(diff_pres_sub); + return ERROR; + } + + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + close(diff_pres_sub); + return OK; + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + close(diff_pres_sub); + return ERROR; + } +} diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h new file mode 100644 index 000000000..71c701fc2 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.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 gyro_calibration.h + * Airspeed sensor calibration routine + */ + +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include <stdint.h> + +int do_airspeed_calibration(int mavlink_fd); + +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp new file mode 100644 index 000000000..3123c4087 --- /dev/null +++ b/src/modules/commander/baro_calibration.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 baro_calibration.cpp + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include <poll.h> +#include <math.h> +#include <fcntl.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_baro.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return ERROR; +} diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h new file mode 100644 index 000000000..bc42bc6cf --- /dev/null +++ b/src/modules/commander/baro_calibration.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 mag_calibration.h + * Barometer calibration routine + */ + +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include <stdint.h> + +int do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h new file mode 100644 index 000000000..fd8b8564d --- /dev/null +++ b/src/modules/commander/calibration_messages.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, 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 calibration_messages.h + * + * Common calibration messages. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef CALIBRATION_MESSAGES_H_ +#define CALIBRATION_MESSAGES_H_ + +#define CAL_STARTED_MSG "%s calibration: started" +#define CAL_DONE_MSG "%s calibration: done" +#define CAL_FAILED_MSG "%s calibration: failed" +#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" + +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" + +#endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp index a26938637..be38ea104 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file calibration_routines.c + * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier <lm@inf.ethz.ch> @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c deleted file mode 100644 index e9d1f3954..000000000 --- a/src/modules/commander/commander.c +++ /dev/null @@ -1,2097 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.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 commander.c - * Main system state machine implementation. - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * - */ - -#include "commander.h" - -#include <nuttx/config.h> -#include <pthread.h> -#include <stdio.h> -#include <stdlib.h> -#include <stdbool.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <debug.h> -#include <sys/prctl.h> -#include <string.h> -#include <drivers/drv_led.h> -#include <drivers/drv_hrt.h> -#include <drivers/drv_tone_alarm.h> -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" -#include <math.h> -#include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/home_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/subsystem_info.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/differential_pressure.h> -#include <mavlink/mavlink_log.h> - -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <systemlib/err.h> - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> -#include <drivers/drv_baro.h> - -#include "calibration_routines.h" -#include "accelerometer_calibration.h" - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - -#include <systemlib/cpuload.h> -extern struct system_load_s system_load; - -/* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -/* File descriptors */ -static int leds; -static int buzzer; -static int mavlink_fd; -static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* pthread loops */ -static void *orb_receive_loop(void *arg); - -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - -static int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -static void buzzer_deinit() -{ - close(buzzer); -} - - -static int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -static void led_deinit() -{ - close(leds); -} - -static int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) -{ - - /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } - - /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } - - /* Trigger Tetris on being bored */ - - return 0; -} - -void tune_confirm(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_error(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) -{ - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) -{ - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - tune_confirm(); - sleep(2); - tune_confirm(); - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_confirm(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_mag); -} - -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - int errcount = 0; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - errcount++; - } - - if (errcount > 1000) { - /* any persisting poll error is a reason to abort */ - mavlink_log_info(mavlink_fd, "permanent gyro error, aborted."); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); - /* set to accel calibration mode */ - status->flag_preflight_airspeed_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - /* exit airspeed calibration mode */ - status->flag_preflight_airspeed_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(diff_pres_sub); -} - - - -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* announce command handling */ - tune_confirm(); - - - /* supported command handling start */ - - /* request to set different system mode */ - switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - /* request to disarm */ - - } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status, mavlink_fd); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - } - break; - - default: { - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); - } - break; - } - - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); - exit(1); -} - -/** - * The daemon 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 commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_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("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("I am in command now!\n"); - - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); - } - - if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; - /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; - /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; - /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (stat_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - mavlink_log_info(mavlink_fd, "system is running"); - - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - - /* Start monitoring loop */ - uint16_t counter = 0; - uint8_t flight_env; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_s global_position; - memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; - - int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - 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; - - while (!thread_should_exit) { - - /* Get current values */ - bool new_data; - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(diff_pres_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); - } - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } - } - - /* update global position estimate */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); - - // } else { - // led_off(LED_AMBER); - // } - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } - } - - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; - bool airspeed_valid = current_status.flag_airspeed_valid; - - /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000) { - current_status.flag_airspeed_valid = true; - - } else { - current_status.flag_airspeed_valid = false; - } - - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; - - } else { - current_status.flag_vector_flight_mode_ok = false; - } - - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid || - airspeed_valid != current_status.flag_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } - - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { - - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_confirm(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if manual stability control modes have to be switched - */ - if (!isfinite(sp_man.manual_sas_switch)) { - - /* this switch is not properly mapped, set default */ - 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) { - - /* 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) { - - /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; - - } else { - /* 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; - } - - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - 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; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position --> arm */ - 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; - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; - - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } - - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } - - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_confirm(); - - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_confirm(); - } - } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - } - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); - state_changed = false; - } - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; - - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(global_position_sub); - close(sensor_sub); - close(cmd_sub); - - warnx("exiting..\n"); - fflush(stdout); - - thread_running = false; - - return 0; -} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp new file mode 100644 index 000000000..ace13bb78 --- /dev/null +++ b/src/modules/commander/commander.cpp @@ -0,0 +1,1872 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * 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 + * 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 commander.cpp + * Main system state machine implementation. + * + */ + +#include <nuttx/config.h> +#include <pthread.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdbool.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <systemlib/err.h> +#include <debug.h> +#include <sys/prctl.h> +#include <sys/stat.h> +#include <string.h> +#include <math.h> +#include <poll.h> + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/safety.h> + +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/cpuload.h> +#include <systemlib/rc_check.h> + +#include "px4_custom_mode.h" +#include "commander_helper.h" +#include "state_machine_helper.h" +#include "calibration_routines.h" +#include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +extern struct system_load_s system_load; + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define MAVLINK_OPEN_INTERVAL 50000 + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 + +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; + +/* Mavlink file descriptors */ +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; +/* if connected via USB */ +static bool on_usb_power = false; + +static float takeoff_alt = 5.0f; + +/* tasks waiting for low prio thread */ +typedef enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task_t; + +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; + +/** + * The daemon 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(). + * + * @ingroup apps + */ +extern "C" __EXPORT int commander_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +void usage(const char *reason); + +/** + * React to commands that are sent e.g. from the mavlink module. + */ +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); + +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +void print_reject_mode(const char *msg); + +void print_reject_arm(const char *msg); + +void print_status(); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + if (!thread_running) + errx(0, "commander already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running"); + print_status(); + + } else { + warnx("\tcommander not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +void print_status() +{ + warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char *armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + close(state_sub); + + + warnx("arming: %s", armed_str); +} + +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* only handle high-priority commands here */ + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (uint8_t) cmd->param1; + uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } + + // TODO remove debug code + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); + /* set arming state */ + arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } + + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } + + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; + + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } + + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } + } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } + + case VEHICLE_CMD_NAV_TAKEOFF: { + if (armed->armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* reject TAKEOFF not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + } + break; + + default: + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + + } + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* send acknowledge command */ + // XXX TODO + } + +} + +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + bool battery_tune_played = false; + bool arm_tune_played = false; + + /* set parameters */ + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + + /* welcome user */ + warnx("starting"); + + /* pthread for slow low prio thread */ + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != OK) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + /* Main state machine */ + /* make sure we are in preflight state */ + memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_DIRECT; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; + + /* neither manual nor offboard control commands have been received */ + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; + + /* mark all signals lost as long as they haven't been found */ + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; + + /* allow manual override initially */ + control_mode.flag_external_manual_override_ok = true; + + /* set battery warning flag */ + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; + + // XXX for now just set sensors as initialized + status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + + /* advertise to ORB */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + /* publish current state machine */ + + /* publish initial state */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + mavlink_log_info(mavlink_fd, "[cmd] started"); + + int ret; + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + + /* Start monitoring loop */ + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; + + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + + uint64_t last_idle_time = 0; + uint64_t start_time = 0; + + bool status_changed = true; + bool param_init_forced = true; + + bool updated = false; + + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + /* Subscribe to differential pressure topic */ + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* Subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + + control_status_leds(&status, &armed, true); + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + start_time = hrt_absolute_time(); + + while (!thread_should_exit) { + + if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* update parameters */ + orb_check(param_changed_sub, &updated); + + if (updated || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!armed.armed) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { + warnx("failed getting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { + control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; + + } else { + control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); + status_changed = true; + + /* re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + } + } + + orb_check(sp_man_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + } + + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); + + /* update safety topic */ + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(safety), safety_sub, &safety); + + // XXX this would be the right approach to do it, but do we *WANT* this? + // /* disarm if safety is now on and still armed */ + // if (safety.safety_switch_available && !safety.safety_off) { + // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // } + } + + /* update global position estimate */ + orb_check(global_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + if (status.condition_local_altitude_valid) { + if (status.condition_landed != local_position.landed) { + status.condition_landed = local_position.landed; + status_changed = true; + + if (status.condition_landed) { + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + + } else { + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + } + } + } + + /* update battery status */ + orb_check(battery_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + // warnx("bat v: %2.2f", battery.voltage_v); + + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } + } + + /* update subsystem */ + orb_check(subsys_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("subsystem changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + status.onboard_control_sensors_health &= ~info.subsystem_type; + } + + status_changed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + + /* check if board is connected via USB */ + //struct stat statbuf; + //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + } + + // XXX remove later + //warnx("bat remaining: %2.2f", status.battery_remaining); + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; + } + + low_voltage_counter++; + + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; + + if (armed.armed) { + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + + } else { + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); + } + + status_changed = true; + } + + critical_voltage_counter++; + + } else { + + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + // XXX check for sensors + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + orb_check(gps_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + /* check if GPS fix is ok */ + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (!home_position_set && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { + /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_positive(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC input check */ + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; + } + } + + status.rc_signal_lost = false; + + transition_result_t res; // store all transitions results here + + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; + + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); + stick_off_counter = 0; + + } else { + stick_off_counter++; + } + + } else { + stick_off_counter = 0; + } + + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + } + + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + if (res == TRANSITION_CHANGED) { + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + } + } + + + /* handle commands last, as the system needs to be updated to handle them */ + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); + + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); + + hrt_abstime t1 = hrt_absolute_time(); + + if (navigation_state_changed || arming_state_changed) { + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic + } + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + } + + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.timestamp = t1; + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t1; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + /* play tune when armed */ + if (tune_arm() == OK) + arm_tune_played = true; + + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) + battery_tune_played = true; + + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) + battery_tune_played = true; + + } else if (battery_tune_played) { + tune_stop(); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + arm_tune_played = false; + } + + fflush(stdout); + counter++; + + int blink_state = blink_msg_state(); + + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; + + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + ret = pthread_join(commander_low_prio_thread, NULL); + + if (ret) { + warn("join failed: %d", ret); + } + + rgbled_set_mode(RGBLED_MODE_OFF); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(local_position_sub); + close(global_position_sub); + close(gps_sub); + close(sensor_sub); + close(safety_sub); + close(cmd_sub); + close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); + + thread_running = false; + + return 0; +} + +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + +void +control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +{ + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + if (set_normal_color) { + /* set color */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + rgbled_set_color(RGBLED_COLOR_AMBER); + } + + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (actuator_armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (actuator_armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } + +#endif + + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) + led_toggle(LED_AMBER); + + } else { + led_off(LED_AMBER); + } + + leds_counter++; +} + +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + print_reject_mode("EASY"); + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "#audio: warning: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "#audio: %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +{ + transition_result_t res = TRANSITION_DENIED; + + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; + } + } + + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } + + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } + + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); + + } else { + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); + } + } + } + + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } + } + + return res; +} + +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; + } +} + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander_low_prio", getpid()); + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; + + while (!thread_should_exit) { + /* wait for up to 200ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); + + /* timed out - periodic check for thread_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + continue; + + /* only handle low-priority commands here */ + switch (cmd.command) { + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot */ + systemreset(false); + + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(100000); + /* reboot to bootloader */ + systemreset(true); + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + int calib_ret = ERROR; + + /* try to go to INIT/PREFLIGHT arming state */ + + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_rc_calibration(mavlink_fd); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } + + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); + + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + + break; + } + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + + if (((int)(cmd.param1)) == 0) { + int ret = param_load_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + + } else if (((int)(cmd.param1)) == 1) { + int ret = param_save_default(); + + if (ret == OK) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); + + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + } + + break; + } + + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } + } + + close(cmd_sub); + + return NULL; +} diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp new file mode 100644 index 000000000..565b4b66a --- /dev/null +++ b/src/modules/commander/commander_helper.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * 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 + * 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 commander_helper.cpp + * Commander helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <stdbool.h> +#include <fcntl.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> +#include <drivers/drv_led.h> +#include <drivers/drv_rgbled.h> + +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#define BLINK_MSG_TIME 700000 // 3 fast blinks + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; +static hrt_abstime blink_msg_end; + +int buzzer_init() +{ + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +} + +void tune_positive() +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); +} + +void tune_neutral() +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_WHITE); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); +} + +void tune_negative() +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); +} + +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + +static int leds; +static int rgbleds; + +int led_init() +{ + blink_msg_end = 0; + + /* first open normal LEDs */ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + /* the blue LED is only available on FMUv1 but not FMUv2 */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + if (ioctl(leds, LED_ON, LED_BLUE)) { + warnx("Blue LED: ioctl fail\n"); + return ERROR; + } + +#endif + + if (ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: ioctl fail\n"); + return ERROR; + } + + /* then try RGB LEDs, this can fail on FMUv1*/ + rgbleds = open(RGBLED_DEVICE_PATH, 0); + + if (rgbleds == -1) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "Unable to open " RGBLED_DEVICE_PATH); +#else + warnx("No RGB LED found"); +#endif + } + + return 0; +} + +void led_deinit() +{ + close(leds); + + if (rgbleds != -1) { + close(rgbleds); + } +} + +int led_toggle(int led) +{ + return ioctl(leds, LED_TOGGLE, led); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +void rgbled_set_color(rgbled_color_t color) +{ + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); +} + +void rgbled_set_mode(rgbled_mode_t mode) +{ + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); +} + +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); +} + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; + return ret; +} diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h new file mode 100644 index 000000000..e9514446c --- /dev/null +++ b/src/modules/commander/commander_helper.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.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 commander_helper.h + * Commander helper functions definitions + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <drivers/drv_rgbled.h> + + +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); + +int buzzer_init(void); +void buzzer_deinit(void); + +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_low_bat(void); +int tune_critical_bat(void); +void tune_stop(void); +int blink_msg_state(); + +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); + +void rgbled_set_color(rgbled_color_t color); +void rgbled_set_mode(rgbled_mode_t mode); +void rgbled_set_pattern(rgbled_pattern_t *pattern); + +/** + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c index 04b4e72ab..40d0386d5 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander_params.c @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * 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 @@ -36,23 +33,23 @@ ****************************************************************************/ /** - * @file commander.h - * Main system state machine definition. + * @file commander_params.c + * + * Parameters defined by the sensors task. * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -void tune_confirm(void); -void tune_error(void); +#include <nuttx/config.h> +#include <systemlib/param/param.h> -#endif /* COMMANDER_H_ */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp new file mode 100644 index 000000000..6e72cf0d9 --- /dev/null +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 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 commander_tests.cpp + * Commander unit tests. Run the tests as follows: + * nsh> commander_tests + * + */ + +#include <systemlib/err.h> + +#include "state_machine_helper_test.h" + +extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); + + +int commander_tests_main(int argc, char *argv[]) +{ + state_machine_helper_test(); + //state_machine_test(); + + return 0; +} diff --git a/src/modules/commander/commander_tests/module.mk b/src/modules/commander/commander_tests/module.mk new file mode 100644 index 000000000..4d10275d1 --- /dev/null +++ b/src/modules/commander/commander_tests/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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = commander_tests +SRCS = commander_tests.cpp \ + state_machine_helper_test.cpp \ + ../state_machine_helper.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 000000000..40bedd9f3 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 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 state_machine_helper_test.cpp + * System state machine unit test. + * + */ + +#include "state_machine_helper_test.h" + +#include "../state_machine_helper.h" +#include <unit_test/unit_test.h> + +class StateMachineHelperTest : public UnitTest +{ +public: + StateMachineHelperTest(); + virtual ~StateMachineHelperTest(); + + virtual const char* run_tests(); + +private: + const char* arming_state_transition_test(); + const char* arming_state_transition_arm_disarm_test(); + const char* main_state_transition_test(); + const char* is_safe_test(); +}; + +StateMachineHelperTest::StateMachineHelperTest() { +} + +StateMachineHelperTest::~StateMachineHelperTest() { +} + +const char* +StateMachineHelperTest::arming_state_transition_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // Identical states. + status.arming_state = ARMING_STATE_INIT; + new_arming_state = ARMING_STATE_INIT; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + + // INIT to STANDBY. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = true; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("transition: init to standby", + TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("ready to arm", armed.ready_to_arm); + + // INIT to STANDBY, sensors not initialized. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = false; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("no transition: sensors not initialized", + TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("not ready to arm", !armed.ready_to_arm); + + return 0; +} + +const char* +StateMachineHelperTest::arming_state_transition_arm_disarm_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // TODO(sjwilks): ARM then DISARM. + return 0; +} + +const char* +StateMachineHelperTest::main_state_transition_test() +{ + struct vehicle_status_s current_state; + main_state_t new_main_state; + + // Identical states. + current_state.main_state = MAIN_STATE_MANUAL; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // AUTO to MANUAL. + current_state.main_state = MAIN_STATE_AUTO; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("transition changed: auto to manual", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to SEATBELT. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = true; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("tranisition: manual to seatbelt", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + + // MANUAL to SEATBELT, invalid local altitude. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = false; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("no transition: invalid local altitude", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to EASY. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = true; + new_main_state = MAIN_STATE_EASY; + mu_assert("transition: manual to easy", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + + // MANUAL to EASY, invalid local position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = false; + new_main_state = MAIN_STATE_EASY; + mu_assert("no transition: invalid position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to AUTO. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = true; + new_main_state = MAIN_STATE_AUTO; + mu_assert("transition: manual to auto", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + + // MANUAL to AUTO, invalid global position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = false; + new_main_state = MAIN_STATE_AUTO; + mu_assert("no transition: invalid global position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + return 0; +} + +const char* +StateMachineHelperTest::is_safe_test() +{ + struct vehicle_status_s current_state; + struct safety_s safety; + struct actuator_armed_s armed; + + armed.armed = false; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + + armed.armed = false; + armed.lockdown = true; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = false; + safety.safety_off = false; + mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + + return 0; +} + +const char* +StateMachineHelperTest::run_tests() +{ + mu_run_test(arming_state_transition_test); + mu_run_test(arming_state_transition_arm_disarm_test); + mu_run_test(main_state_transition_test); + mu_run_test(is_safe_test); + + return 0; +} + +void +state_machine_helper_test() +{ + StateMachineHelperTest* test = new StateMachineHelperTest(); + test->UnitTest::print_results(test->run_tests()); +} diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 000000000..10a68e602 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 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 state_machine_helper_test.h + */ + +#ifndef STATE_MACHINE_HELPER_TEST_H_ +#define STATE_MACHINE_HELPER_TEST_ + +void state_machine_helper_test(); + +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp new file mode 100644 index 000000000..30cd0d48d --- /dev/null +++ b/src/modules/commander/gyro_calibration.cpp @@ -0,0 +1,305 @@ +/**************************************************************************** + * + * 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 gyro_calibration.cpp + * + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "calibration_messages.h" +#include "commander_helper.h" + +#include <stdio.h> +#include <fcntl.h> +#include <poll.h> +#include <math.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_gyro.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "gyro"; + +int do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); + + struct gyro_scale gyro_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } + + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; + + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } + + close(sub_sensor_gyro); + + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; + } + + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; + } + } + + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + res = ERROR; + } + } + +#if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); + tune_neutral(); + + /* scale calibration */ + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ + + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); + + close(fd); + + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + + if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) { + mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); + close(sub_sensor_combined); + return OK; + } + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag > M_PI_F) mag -= 2 * M_PI_F; + + if (mag < -M_PI_F) mag += 2 * M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2 * M_PI_F; + + if (diff < -M_PI_F) diff += 2 * M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); + + + if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); + close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); + return ERROR; + } + + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); + tune_neutral(); + +#endif + + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); + res = ERROR; + } + } + + if (res == OK) { + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } + + return res; +} diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h new file mode 100644 index 000000000..59c32a15e --- /dev/null +++ b/src/modules/commander/gyro_calibration.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 gyro_calibration.h + * Gyroscope calibration routine + */ + +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include <stdint.h> + +int do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp new file mode 100644 index 000000000..4ebf266f4 --- /dev/null +++ b/src/modules/commander/mag_calibration.cpp @@ -0,0 +1,285 @@ +/**************************************************************************** + * + * 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 mag_calibration.cpp + * + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" +#include "calibration_messages.h" + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <poll.h> +#include <math.h> +#include <fcntl.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_mag.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static const char *sensor_name = "mag"; + +int do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 500 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter; + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* erase old calibration */ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } + + if (res == OK) { + /* calibrate range */ + res = ioctl(fd, MAGIOCCALIBRATE, fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "Skipped scale calibration"); + /* this is non-fatal - mark it accordingly */ + res = OK; + } + } + + close(fd); + + float *x = NULL; + float *y = NULL; + float *z = NULL; + + if (res == OK) { + /* allocate memory */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); + + x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); + + if (x == NULL || y == NULL || z == NULL) { + mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + res = ERROR; + return res; + } + } else { + /* exit */ + return ERROR; + } + + if (res == OK) { + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; + + mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + + calibration_counter = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + calibration_counter++; + + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } + + close(sub_mag); + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + if (res == OK) { + + /* sphere fit */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); + + if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { + mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + res = ERROR; + } + } + + if (x != NULL) + free(x); + + if (y != NULL) + free(y); + + if (z != NULL) + free(z); + + if (res == OK) { + /* apply calibration and set parameters */ + struct mag_scale mscale; + + fd = open(MAG_DEVICE_PATH, 0); + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + } + + if (res == OK) { + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + close(fd); + + if (res == OK) { + /* set parameters */ + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + res = ERROR; + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + } + + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); + } + + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } + + mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } + } + + return res; +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h new file mode 100644 index 000000000..a101cd7b1 --- /dev/null +++ b/src/modules/commander/mag_calibration.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 mag_calibration.h + * Magnetometer calibration routine + */ + +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include <stdint.h> + +int do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955a..554dfcb08 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,8 +36,14 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c - +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h new file mode 100644 index 000000000..b60a7e0b9 --- /dev/null +++ b/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,37 @@ +/* + * px4_custom_mode.h + * + * Created on: 09.08.2013 + * Author: ton + */ + +#ifndef PX4_CUSTOM_MODE_H_ +#define PX4_CUSTOM_MODE_H_ + +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; +}; + +#endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp new file mode 100644 index 000000000..90ede499a --- /dev/null +++ b/src/modules/commander/rc_calibration.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 rc_calibration.cpp + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include <poll.h> +#include <unistd.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + bool changed; + orb_check(sub_man, &changed); + + if (!changed) { + mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + return ERROR; + } + + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + close(sub_man); + return ERROR; + } + + mavlink_log_info(mavlink_fd, "trim calibration done"); + close(sub_man); + return OK; +} diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h new file mode 100644 index 000000000..9aa6faafa --- /dev/null +++ b/src/modules/commander/rc_calibration.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 rc_calibration.h + * Remote Control calibration routine + */ + +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include <stdint.h> + +int do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c deleted file mode 100644 index ab728c7bb..000000000 --- a/src/modules/commander/state_machine_helper.c +++ /dev/null @@ -1,757 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.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 state_machine_helper.c - * State machine helper functions implementations - */ - -#include <stdio.h> -#include <unistd.h> - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/systemlib.h> -#include <drivers/drv_hrt.h> -#include <mavlink/mavlink_log.h> - -#include "state_machine_helper.h" - -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; - int ret = ERROR; - - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); - - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } - } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; - } - - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; - } - - return ret; -} - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - - /* XXX allow arming by external components on multicopters only if not yet armed by RC */ - /* XXX allow arming only if core sensors are ok */ - armed.ready_to_arm = true; - - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp new file mode 100644 index 000000000..490fc8fc6 --- /dev/null +++ b/src/modules/commander/state_machine_helper.cpp @@ -0,0 +1,708 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.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 state_machine_helper.cpp + * State machine helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <stdbool.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <mavlink/mavlink_log.h> + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + +transition_result_t +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) +{ + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { + + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = true; + } + } + + break; + + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = true; + } + + break; + + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY_ERROR: + + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + + /* end of atomic state update */ + irqrestore(flags); + + if (ret == TRANSITION_DENIED) + warnx("arming transition rejected"); + + return ret; +} + +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + + } else { + return false; + } +} + +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} + +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; + + case MAIN_STATE_SEATBELT: + + /* need at minimum altitude estimate */ + if (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_EASY: + + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; + main_state_changed = true; + } + } + + return ret; +} + +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_LOITER: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_MISSION: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_RTL: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed state */ + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; + navigation_state_changed = true; + } + } + + return ret; +} + +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} + +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + +/** +* Transition from one hil state to another +*/ +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; + + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + + // XXX also set lockdown here + + ret = OK; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc729..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -46,164 +46,34 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/safety.h> +#include <uORB/topics/vehicle_control_mode.h> -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +} transition_result_t; -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); +bool check_arming_state_changed(); -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +bool check_main_state_changed(); -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); +bool check_navigation_state_changed(); +void set_navigation_state_changed(); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d..36bc8c24b 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template<class T> -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase +{ +public: + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + float get() { return _val; } + void set(float val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + int get() { return _val; } + void set(int val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702..66e929038 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam<float> _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam<float> _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam<float> _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam<float> _kP; - BlockParam<float> _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam<float> _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam<float> _mean; - BlockParam<float> _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 9c0720aa5..46dc1bec2 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -58,7 +58,7 @@ #include <poll.h> extern "C" { -#include <systemlib/geo/geo.h> +#include <geo/geo.h> } #include "../blocks.hpp" diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 6c9c137bb..b6b4546c2 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -53,6 +53,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> @@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&global_pos, 0, sizeof(global_pos)); struct manual_control_setpoint_s manual_sp; memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); @@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; @@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* control */ - - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + /* set manual setpoints if required */ + if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (vstatus.rc_signal_lost) { @@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - /* pass through flaps */ if (isfinite(manual_sp.flaps)) { actuators.control[4] = manual_sp.flaps; @@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[4] = 0.0f; } - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else { /* directly pass through values */ actuators.control[0] = manual_sp.roll; /* positive pitch means negative actuator -> pull up */ @@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } } } + + /* execute attitude control if requested */ + if (control_mode.flag_control_attitude_enabled) { + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; + + } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f655a13bd..d65045d68 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -156,7 +156,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -166,8 +166,8 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || + _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); @@ -219,89 +219,83 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { + if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; - - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between - // the min/max velocity - float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + - _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); - - // pitch channel -> rate of climb - // TODO, might want to put a gain on this, otherwise commanding - // from +1 -> -1 m/s for rate of climb - //float dThrottle = _cr2Thr.update( - //_crMax.get()*_manual.pitch - _pos.vz); - - // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); - - // throttle channel -> velocity - // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - - // currently using manual throttle - // XXX if you enable this watch out, vz might be very noisy - //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; - - // XXX limit throttle to manual setting (safety) for now. - // If it turns out to be confusing, it can be removed later once - // a first binary release can be targeted. - // This is not a hack, but a design choice. - - /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } - } + } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // pitch channel -> rate of climb + // TODO, might want to put a gain on this, otherwise commanding + // from +1 -> -1 m/s for rate of climb + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); - // body rates controller, disabled for now - else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { + // roll channel -> bank angle + float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.roll); - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + // throttle channel -> velocity + // negative sign because nose over to increase speed + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + // yaw rate cmd + float rCmd = 0; + + // stabilization + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + + // currently using manual throttle + // XXX if you enable this watch out, vz might be very noisy + //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.control[CH_THR] = _manual.throttle; + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (_status.hil_state != HIL_STATE_ON) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; } + // body rates controller, disabled for now + // TODO + } else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp new file mode 100644 index 000000000..00a0dcd61 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -0,0 +1,781 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_att_control_main.c + * Implementation of a generic attitude controller based on classic orthogonal PIDs. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +#include <ecl/attitude_fw/ecl_pitch_controller.h> +#include <ecl/attitude_fw/ecl_roll_controller.h> +#include <ecl/attitude_fw/ecl_yaw_controller.h> + +/** + * Fixedwing attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); + +class FixedwingAttitudeControl +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_roll_feedforward; + float y_integrator_max; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t tconst; + param_t p_p; + param_t p_d; + param_t p_i; + param_t p_rmax_pos; + param_t p_rmax_neg; + param_t p_integrator_max; + param_t p_roll_feedforward; + param_t r_p; + param_t r_d; + param_t r_i; + param_t r_integrator_max; + param_t r_rmax; + param_t y_p; + param_t y_i; + param_t y_d; + param_t y_roll_feedforward; + param_t y_integrator_max; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingAttitudeControl *g_control; +} + +FixedwingAttitudeControl::FixedwingAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + _attitude_sp_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_valid(false) +{ + _parameter_handles.tconst = param_find("FW_ATT_TC"); + _parameter_handles.p_p = param_find("FW_P_P"); + _parameter_handles.p_d = param_find("FW_P_D"); + _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); + _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); + _parameter_handles.p_integrator_max = param_find("FW_P_IMAX"); + _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); + + _parameter_handles.r_p = param_find("FW_R_P"); + _parameter_handles.r_d = param_find("FW_R_D"); + _parameter_handles.r_i = param_find("FW_R_I"); + _parameter_handles.r_integrator_max = param_find("FW_R_IMAX"); + _parameter_handles.r_rmax = param_find("FW_R_RMAX"); + + _parameter_handles.y_p = param_find("FW_Y_P"); + _parameter_handles.y_i = param_find("FW_Y_I"); + _parameter_handles.y_d = param_find("FW_Y_D"); + _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); + _parameter_handles.y_integrator_max = param_find("FW_Y_IMAX"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +FixedwingAttitudeControl::parameters_update() +{ + + param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.p_p, &(_parameters.p_p)); + param_get(_parameter_handles.p_d, &(_parameters.p_d)); + param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); + param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + + param_get(_parameter_handles.r_p, &(_parameters.r_p)); + param_get(_parameter_handles.r_d, &(_parameters.r_d)); + param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); + + param_get(_parameter_handles.y_p, &(_parameters.y_p)); + param_get(_parameter_handles.y_i, &(_parameters.y_i)); + param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + /* pitch control parameters */ + _pitch_ctrl.set_time_constant(_parameters.tconst); + _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); + _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); + _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); + _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); + _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); + _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + + /* roll control parameters */ + _roll_ctrl.set_time_constant(_parameters.tconst); + _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); + _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); + _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); + _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + + /* yaw control parameters */ + _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); + _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); + _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); + _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); + _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + + return OK; +} + +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ + bool vcontrol_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); + + if (vcontrol_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + } +} + +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +bool +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +FixedwingAttitudeControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + vehicle_control_mode_poll(); + + vehicle_manual_poll(); + + /* lock integrator until control is started */ + bool lock_integrator; + + if (_vcontrol_mode.flag_control_attitude_enabled) { + lock_integrator = false; + + } else { + lock_integrator = true; + } + + /* decide if in stabilized or full manual control */ + + if (_vcontrol_mode.flag_control_attitude_enabled) { + + /* scale from radians to normalized -1 .. 1 range */ + const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); + + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is smaller than min, the sensor is not giving good readings */ + if (!_airspeed_valid || + (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s)) { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + + } else { + airspeed = _airspeed.indicated_airspeed_m_s; + } + + float airspeed_scaling = _parameters.airspeed_trim / airspeed; + //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + throttle_sp = _manual.throttle; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f; + att_sp.thrust = throttle_sp; + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } + } + + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + } + + } else { + /* manual/direct control */ + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.pitch; + _actuators.control[2] = _manual.yaw; + _actuators.control[3] = _manual.throttle; + _actuators.control[4] = _manual.flaps; + } + + _actuators.control[5] = _manual.aux1; + _actuators.control[6] = _manual.aux2; + _actuators.control[7] = _manual.aux3; + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new FixedwingAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c new file mode 100644 index 000000000..97aa275de --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ + +// @DisplayName Attitude Time Constant +// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. +// @Range 0.4 to 1.0 seconds, in tens of seconds +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); + +// @DisplayName Proportional gain. +// @Description This defines how much the elevator input will be commanded dependend on the current pitch error. +// @Range 10 to 200, 1 increments +PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); + +// @DisplayName Damping gain. +// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. +// @Range 0.0 to 10.0, 0.1 increments +PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); + +// @DisplayName Integrator gain. +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. +// @Range 0 to 50.0 +PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); + +// @DisplayName Maximum positive / up pitch rate. +// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); + +// @DisplayName Maximum negative / down pitch rate. +// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); + +// @DisplayName Pitch Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); + +// @DisplayName Roll feedforward gain. +// @Description This compensates during turns and ensures the nose stays level. +// @Range 0.5 2.0 +// @Increment 0.05 +// @User User +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); + +// @DisplayName Proportional Gain. +// @Description This gain controls the roll angle to roll actuator output. +// @Range 10.0 200.0 +// @Increment 10.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); + +// @DisplayName Damping Gain +// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. +// @Range 0.0 10.0 +// @Increment 1.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); + +// @DisplayName Integrator Gain +// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. +// @Range 0.0 100.0 +// @Increment 5.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); + +// @DisplayName Roll Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); + +// @DisplayName Maximum Roll Rate +// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); + + +PARAM_DEFINE_FLOAT(FW_Y_P, 0); +PARAM_DEFINE_FLOAT(FW_Y_I, 0); +PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_Y_D, 0); +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk new file mode 100644 index 000000000..1e600757e --- /dev/null +++ b/src/modules/fw_att_control/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. +# +############################################################################ + +# +# Fixedwing attitude control application +# + +MODULE_COMMAND = fw_att_control + +SRCS = fw_att_control_main.cpp \ + fw_att_control_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp new file mode 100644 index 000000000..a9648b207 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -0,0 +1,1217 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_pos_control_l1_main.c + * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll + * angle, equivalent to a lateral motion (for copters and rovers). + * + * Original publication for horizontal control class: + * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * + * More details and acknowledgements in the referenced library headers. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +#include <ecl/l1/ecl_l1_pos_controller.h> +#include <external_lgpl/tecs/tecs.h> + +/** + * L1 control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); + +class FixedwingPositionControl +{ +public: + /** + * Constructor + */ + FixedwingPositionControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingPositionControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _global_set_triplet_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _control_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _accel_sub; /**< body frame accelerations */ + + orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct accel_report _accel; /**< body frame accelerations */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + float _launch_lat; + float _launch_lon; + float _launch_alt; + bool _launch_valid; + + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + + /* throttle and airspeed states */ + float _airspeed_error; ///< airspeed error to setpoint in m/s + bool _airspeed_valid; ///< flag if a valid airspeed estimate exists + uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures + float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s + bool _global_pos_valid; ///< global position is valid + math::Dcm _R_nb; ///< current attitude + + ECL_L1_Pos_Controller _l1_control; + TECS _tecs; + + struct { + float l1_period; + float l1_damping; + + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float pitch_limit_min; + float pitch_limit_max; + float roll_limit; + float throttle_min; + float throttle_max; + float throttle_cruise; + + float throttle_land_max; + + float loiter_hold_radius; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t l1_period; + param_t l1_damping; + + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + + param_t pitch_limit_min; + param_t pitch_limit_max; + param_t roll_limit; + param_t throttle_min; + param_t throttle_max; + param_t throttle_cruise; + + param_t throttle_land_max; + + param_t loiter_hold_radius; + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_control_mode_poll(); + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Control position. + */ + bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet); + + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace l1_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingPositionControl *g_control; +} + +FixedwingPositionControl::FixedwingPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _global_set_triplet_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), +/* states */ + _setpoint_valid(false), + _loiter_hold(false), + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false), + land_noreturn(false) +{ + _nav_capabilities.turn_distance = 0.0f; + + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); + _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); + _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); + _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); + _parameter_handles.roll_limit = param_find("FW_R_LIM"); + _parameter_handles.throttle_min = param_find("FW_THR_MIN"); + _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingPositionControl::~FixedwingPositionControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + l1_control::g_control = nullptr; +} + +int +FixedwingPositionControl::parameters_update() +{ + + /* L1 control parameters */ + param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); + param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); + param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); + param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); + param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit)); + param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); + param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); + param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + _l1_control.set_l1_damping(_parameters.l1_damping); + _l1_control.set_l1_period(_parameters.l1_period); + _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + + /* sanity check parameters */ + if (_parameters.airspeed_max < _parameters.airspeed_min || + _parameters.airspeed_max < 5.0f || + _parameters.airspeed_min > 100.0f || + _parameters.airspeed_trim < _parameters.airspeed_min || + _parameters.airspeed_trim > _parameters.airspeed_max) { + warnx("error: airspeed parameters invalid"); + return 1; + } + + return OK; +} + +void +FixedwingPositionControl::vehicle_control_mode_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &vstatus_updated); + + if (vstatus_updated) { + + bool was_armed = _control_mode.flag_armed; + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + if (!was_armed && _control_mode.flag_armed) { + _launch_lat = _global_pos.lat / 1e7f; + _launch_lon = _global_pos.lon / 1e7f; + _launch_alt = _global_pos.alt; + _launch_valid = true; + } + } +} + +bool +FixedwingPositionControl::vehicle_airspeed_poll() +{ + /* check if there is an airspeed update or if it timed out */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); + + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } + } + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + + return airspeed_updated; +} + +void +FixedwingPositionControl::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _R_nb(i, j) = _att.R[i][j]; + } +} + +void +FixedwingPositionControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingPositionControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool global_sp_updated; + orb_check(_global_set_triplet_sub, &global_sp_updated); + + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); + _setpoint_valid = true; + } +} + +void +FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + l1_control::g_control->task_main(); +} + +float +FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) +{ + float airspeed; + + if (_airspeed_valid) { + airspeed = _airspeed.true_airspeed_m_s; + + } else { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + } + + /* cruise airspeed for all modes unless modified below */ + float target_airspeed = airspeed_demand; + + /* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */ + target_airspeed += _groundspeed_undershoot; + + if (0/* throttle nudging enabled */) { + //target_airspeed += nudge term. + } + + /* sanity check: limit to range */ + target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max); + + /* plain airspeed error */ + _airspeed_error = target_airspeed - airspeed; + + return target_airspeed; +} + +void +FixedwingPositionControl::calculate_gndspeed_undershoot() +{ + + if (_global_pos_valid) { + /* get ground speed vector */ + math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); + + /* rotate with current attitude */ + math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + yaw_vector.normalize(); + float ground_speed_body = yaw_vector * ground_speed_vector; + + /* + * Ground speed undershoot is the amount of ground velocity not reached + * by the plane. Consequently it is zero if airspeed is >= min ground speed + * and positive if airspeed < min ground speed. + * + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. + */ + _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + + } else { + _groundspeed_undershoot = 0; + } +} + +bool +FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet) +{ + bool setpoint = true; + + calculate_gndspeed_undershoot(); + + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + + // XXX re-visit + float baro_altitude = _global_pos.alt; + + /* filter speed and altitude for controller */ + math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + + _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + + /* no throttle limit as default */ + float throttle_max = 1.0f; + + /* AUTONOMOUS FLIGHT */ + + // XXX this should only execute if auto AND safety off (actuators active), + // else integrators should be constantly reset. + if (_control_mode.flag_control_position_enabled) { + + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + + /* execute navigation once we have a setpoint */ + if (_setpoint_valid) { + + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + + /* previous waypoint */ + math::Vector2f prev_wp; + + if (global_triplet.previous_valid) { + prev_wp.setX(global_triplet.previous.lat / 1e7f); + prev_wp.setY(global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(global_triplet.current.lat / 1e7f); + prev_wp.setY(global_triplet.current.lon / 1e7f); + + } + + // XXX add RTL switch + if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + + math::Vector2f rtl_pos(_launch_lat, _launch_lon); + + _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + // XXX handle case when having arrived at home (loiter) + + } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + global_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -5.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + + if (altitude_error > -4.0f) { + + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, _parameters.throttle_max, throttle_land, + math::radians(-10.0f), math::radians(15.0f)); + + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else { + + /* normal cruise speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + + } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { + + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + } + + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else if (_control_mode.flag_armed) { + + /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ + + // XXX rework with smarter state machine + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt + 25.0f; + _loiter_hold = true; + } + + altitude_error = _loiter_hold_alt - _global_pos.alt; + + math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); + + /* loiter around current position */ + _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, + 1, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* climb with full throttle if the altitude error is bigger than 5 meters */ + bool climb_out = (altitude_error > 3); + + float min_pitch; + + if (climb_out) { + min_pitch = math::radians(20.0f); + + } else { + min_pitch = math::radians(_parameters.pitch_limit_min); + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + climb_out, min_pitch, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + if (climb_out) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + } + } + + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + + if (0/* switched from another mode to seatbelt */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* seatbelt on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + climb_out, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + setpoint = false; + } + + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + + return setpoint; +} + +void +FixedwingPositionControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_control_mode_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + /* abort on a nonzero return value from the parameter init */ + if (parameters_update()) { + /* parameter setup went wrong, abort */ + warnx("aborting startup due to errors."); + _task_should_exit = true; + } + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_control_mode_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + // XXX add timestamp check + _global_pos_valid = true; + + vehicle_attitude_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_airspeed_poll(); + // vehicle_baro_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* + * Attempt to control position, on success (= sensors present and not in manual mode), + * publish setpoint. + */ + if (control_position(current_position, ground_speed, _global_triplet)) { + _att_sp.timestamp = hrt_absolute_time(); + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_pos_control_l1", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4048, + (main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_pos_control_l1_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (l1_control::g_control != nullptr) + errx(1, "already running"); + + l1_control::g_control = new FixedwingPositionControl; + + if (l1_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != l1_control::g_control->start()) { + delete l1_control::g_control; + l1_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (l1_control::g_control == nullptr) + errx(1, "not running"); + + delete l1_control::g_control; + l1_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (l1_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c new file mode 100644 index 000000000..3bb872405 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); + + +PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); + + +PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); + + +PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + + +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + + +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + + +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + + +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk new file mode 100644 index 000000000..b00b9aa5a --- /dev/null +++ b/src/modules/fw_pos_control_l1/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. +# +############################################################################ + +# +# Fixedwing L1 position control application +# + +MODULE_COMMAND = fw_pos_control_l1 + +SRCS = fw_pos_control_l1_main.cpp \ + fw_pos_control_l1_params.c diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1aef739c7..d383146f9 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> #include <poll.h> #include <drivers/drv_gpio.h> #include <modules/px4iofirmware/protocol.h> @@ -62,6 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -109,19 +112,19 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[3], "a1")) { use_io = true; - pin = PX4IO_ACC1; + pin = PX4IO_P_SETUP_RELAYS_ACC1; } else if (!strcmp(argv[3], "a2")) { use_io = true; - pin = PX4IO_ACC2; + pin = PX4IO_P_SETUP_RELAYS_ACC2; } else if (!strcmp(argv[3], "r1")) { use_io = true; - pin = PX4IO_RELAY1; + pin = PX4IO_P_SETUP_RELAYS_POWER1; } else if (!strcmp(argv[3], "r2")) { use_io = true; - pin = PX4IO_RELAY2; + pin = PX4IO_P_SETUP_RELAYS_POWER2; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -142,7 +145,7 @@ int gpio_led_main(int argc, char *argv[]) char pin_name[24]; if (use_io) { - if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); } else { @@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); + /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 919d01561..7c10e297b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> +#include <commander/px4_custom_mode.h> #include "waypoints.h" #include "orb_topics.h" @@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ - if (v_status.flag_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + if (v_status.hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* arming state */ + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ - if (armed.armed) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - } - - switch (v_status.state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - - } else { - *mavlink_state = MAV_STATE_UNINIT; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_AUTO: + } + *mavlink_custom_mode = custom_mode.data; + + /** + * Set mavlink state + **/ + + /* set calibration state */ + if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } - } @@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: @@ -534,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; @@ -549,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[]) case 'b': baudrate = strtoul(optarg, NULL, 10); - if (baudrate == 0) + if (baudrate < 9600 || baudrate > 921600) errx(1, "invalid baud rate '%s'", optarg); break; @@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } @@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.load * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, @@ -703,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); @@ -751,9 +738,12 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + //mavlink_logbuffer_destroy(&lb); + thread_running = false; - exit(0); + return 0; } static void @@ -777,7 +767,7 @@ int mavlink_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "mavlink already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink", @@ -786,15 +776,25 @@ int mavlink_main(int argc, char *argv[]) 2048, mavlink_thread_main, (const char **)argv); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "mavlink already stopped"); + thread_should_exit = true; while (thread_running) { usleep(200000); - printf("."); + warnx("."); } warnx("terminated."); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..c51a6de08 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include <mqueue.h> #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -67,6 +71,7 @@ #include <systemlib/err.h> #include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> +#include <commander/px4_custom_mode.h> __BEGIN_DECLS @@ -97,11 +102,22 @@ struct vehicle_global_position_s hil_global_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; +struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; +static orb_advert_t pub_hil_battery = -1; + +/* packet counter */ +static int hil_counter = 0; +static int hil_frames = 0; +static uint64_t old_timestamp = 0; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; @@ -140,7 +156,8 @@ handle_message(mavlink_message_t *msg) vcmd.param5 = cmd_mavlink.param5; vcmd.param6 = cmd_mavlink.param6; vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; vcmd.target_system = cmd_mavlink.target_system; vcmd.target_component = cmd_mavlink.target_component; vcmd.source_system = msg->sysid; @@ -150,6 +167,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -188,15 +206,17 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; vcmd.param6 = 0; vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; vcmd.target_system = new_mode.target_system; vcmd.target_component = MAV_COMP_ID_ALL; vcmd.source_system = msg->sysid; @@ -349,11 +369,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - /* sensors general */ hil_sensors.timestamp = hrt_absolute_time(); @@ -380,9 +395,9 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; /* magnetometer */ float mga2ga = 1.0e-3f; @@ -407,41 +422,140 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_counter = hil_counter; /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa hil_sensors.differential_pressure_counter = hil_counter; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); - float ias = calc_indicated_airspeed(imu.diff_pressure); + float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + + accel.x_raw = imu.xacc / mg2ms2; + + accel.y_raw = imu.yacc / mg2ms2; + + accel.z_raw = imu.zacc / mg2ms2; + + accel.x = imu.xacc; + + accel.y = imu.yacc; + + accel.z = imu.zacc; + + accel.temperature = imu.temperature; + + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + + mag.x_raw = imu.xmag / mga2ga; + + mag.y_raw = imu.ymag / mga2ga; + + mag.z_raw = imu.zmag / mga2ga; + + mag.x = imu.xmag; + + mag.y = imu.ymag; + + mag.z = imu.zmag; + + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + + baro.pressure = imu.abs_pressure; + + baro.altitude = imu.pressure_alt; + + baro.temperature = imu.temperature; + + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + // increment counters hil_counter++; hil_frames++; // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames / 10); old_timestamp = timestamp; hil_frames = 0; } @@ -466,9 +580,11 @@ handle_message(mavlink_message_t *msg) /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -481,6 +597,7 @@ handle_message(mavlink_message_t *msg) /* publish GPS measurement data */ if (pub_hil_gps > 0) { orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } @@ -499,10 +616,12 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + hil_global_pos.valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -515,6 +634,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } @@ -526,8 +646,8 @@ handle_message(mavlink_message_t *msg) /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); - + hil_attitude.R[i][j] = C_nb(i, j); + hil_attitude.R_valid = true; /* set quaternion */ @@ -549,9 +669,48 @@ handle_message(mavlink_message_t *msg) if (pub_hil_attitude > 0) { orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + + accel.x = hil_state.xacc; + + accel.y = hil_state.yacc; + + accel.z = hil_state.zacc; + + accel.temperature = 25.0f; + + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { @@ -618,17 +777,23 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -636,10 +801,10 @@ receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* Handle packet with waypoint component */ + /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - /* Handle packet with parameter component */ + /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } @@ -660,6 +825,7 @@ receive_start(int uart) fcntl(uart, F_SETFL, flags | O_NONBLOCK); struct sched_param param; + (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); param.sched_priority = SCHED_PRIORITY_MAX - 40; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); @@ -667,5 +833,7 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + + pthread_attr_destroy(&receiveloop_attr); return thread; } diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index be88b8794..d37b18a19 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -167,9 +167,31 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->loiter_direction = (param3 >= 0) ? 1 : -1; sp->param1 = param1; - sp->param1 = param2; - sp->param1 = param3; - sp->param1 = param4; + sp->param2 = param2; + sp->param3 = param3; + sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -198,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d38..5d3d6a73c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index edb8761b8..f6860930c 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,11 +67,13 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -98,6 +100,8 @@ struct listener { uintptr_t arg; }; +uint16_t cm_uint16_from_m_float(float m); + static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); @@ -112,12 +116,14 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -138,15 +144,30 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + void l_sensor_combined(const struct listener *l) { @@ -232,8 +253,10 @@ l_vehicle_gps_position(const struct listener *l) /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; @@ -244,10 +267,10 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 + cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ @@ -272,19 +295,23 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.state_machine, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -334,7 +361,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, @@ -470,8 +497,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -488,7 +516,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -502,7 +530,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -516,7 +544,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -530,7 +558,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } @@ -562,28 +590,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -632,19 +686,32 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; + + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); +} + +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); } static void * uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, @@ -673,7 +740,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); @@ -768,7 +835,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ @@ -783,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); @@ -792,5 +867,7 @@ uorb_receive_start(void) pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + + pthread_attr_destroy(&uorb_attr); return thread; } diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 73e278dc6..91773843b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -55,15 +55,19 @@ #include <uORB/topics/vehicle_global_position_set_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> +#include <uORB/topics/battery_status.h> #include <drivers/drv_rc_input.h> +#include <uORB/topics/navigation_capabilities.h> struct mavlink_subscriptions { int sensor_sub; @@ -75,8 +79,10 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; + int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; @@ -87,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -97,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a88..5e5ee8261 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a17..7e4a2688f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -246,64 +246,19 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - /* * Calculate distance in global frame. * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - //TODO: implement for z once altidude contoller is implemented - - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,20 +270,24 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -337,17 +296,27 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); - return sqrt(dx * dx + dy * dy + dz * dz); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); } else { return -1.0f; } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { + /* nothing to check here, return */ + return; + } + if (wpm->current_active_wp_id < wpm->size) { float orbit; @@ -366,38 +335,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - 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); + 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, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_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); + 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, &dist_xy, &dist_z); } 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); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; - } -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,28 +390,77 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; } if (time_elapsed) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + if (cur_wp->autocontinue) { + cur_wp->current = 0; + float navigation_lat = -1.0f; + float navigation_lon = -1.0f; + float navigation_alt = -1.0f; + int navigation_frame = -1; + + /* initialize to current position in case we don't find a suitable navigation waypoint */ + if (global_pos->valid) { + navigation_lat = global_pos->lat/1e7; + navigation_lon = global_pos->lon/1e7; + navigation_alt = global_pos->alt; + navigation_frame = MAV_FRAME_GLOBAL; + } else if (local_pos->xy_valid && local_pos->z_valid) { + navigation_lat = local_pos->x; + navigation_lon = local_pos->y; + navigation_alt = local_pos->z; + navigation_frame = MAV_FRAME_LOCAL_NED; + } + + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning - */ - wpm->current_active_wp_id = 0; + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { + + /* this is a navigation waypoint */ + navigation_frame = cur_wp->frame; + navigation_lat = cur_wp->x; + navigation_lon = cur_wp->y; + navigation_alt = cur_wp->z; + } + + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } + + /* we risk an endless loop for missions without navigation waypoints, abort. */ + break; } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) @@ -465,7 +492,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -488,12 +515,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -504,115 +526,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; @@ -623,26 +536,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -670,13 +573,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); -#endif wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); @@ -684,33 +582,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->timestamp_firstinside_orbit = 0; } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -1157,5 +1038,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd30..d7d6b31dc 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include <stdbool.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/navigation_capabilities.h> // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 20fb11b2c..0edb53a3e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -167,12 +167,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", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf if (strcmp(uart_name, "/dev/ttyACM0") != 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); + warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf /* 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)", 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); + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); close(uart); return -1; } @@ -273,18 +273,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// case SYSTEM_STATE_MANUAL: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; +// break; +// +// case SYSTEM_STATE_STABILIZED: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; +// break; +// +// case SYSTEM_STATE_AUTO: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; +// break; +// +// case SYSTEM_STATE_MISSION_ABORT: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } + } /** @@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ @@ -430,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.load * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, @@ -473,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n" + " mavlink_onboard stop\n" + " mavlink_onboard status\n"); exit(1); } @@ -491,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink_onboard", @@ -508,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[]) while (thread_running) { usleep(200000); } - warnx("terminated."); + warnx("terminated"); exit(0); } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 68d49c24b..4658bcc1d 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); + warnx("terminating..."); fflush(stdout); usleep(50000); @@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* create command */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } - do { - nread = read(uart_fd, &ch, 1); + /* non-blocking read. read may return negative values */ + nread = read(uart_fd, buf, sizeof(buf)); - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); } - } while (nread > 0); + } } } @@ -319,12 +333,12 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -}
\ No newline at end of file +} diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..1b49c9ce4 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,9 +52,11 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_rc_input.h> @@ -69,7 +71,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..c84b6fd26 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 99f25cfe9..60a211817 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.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 @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -57,12 +59,13 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <drivers/drv_gyro.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> @@ -74,333 +77,311 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; - -static orb_advert_t actuator_pub; - -static struct vehicle_status_s state; +static const float min_takeoff_throttle = 0.3f; +static const float yaw_deadzone = 0.01f; static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - memset(&state, 0, sizeof(state)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - - /* - * Do not rate-limit the loop to prevent aliasing - * if rate-limiting would be desired later, the line below would - * enable it. - * - * rate-limit the attitude subscription to 200Hz to pace our loop - * orb_set_interval(att_sub, 5); - */ - struct pollfd fds[2] = { - { .fd = att_sub, .events = POLLIN }, - { .fd = param_sub, .events = POLLIN } - }; + /* subscribe */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } - actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; - - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; + bool reset_yaw_sp = true; - /* store if we stopped a yaw movement */ - bool first_time_after_yaw_speed_control = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } else if (ret == 0) { - /* no return value, ignore */ - } else { + } else if (ret > 0) { + /* only run controller if attitude changed */ + perf_begin(mc_loop_perf); - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + bool updated; + + /* parameters */ + orb_check(parameter_update_sub, &updated); + + if (updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ - // XXX no params here yet } - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { + /* control mode */ + orb_check(vehicle_control_mode_sub, &updated); - perf_begin(mc_loop_perf); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); + } - /* get a local copy of system state */ - bool updated; - orb_check(state_sub, &updated); + /* manual control setpoint */ + orb_check(manual_control_setpoint_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - } + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + } - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - /* get a local copy of rates setpoint */ - orb_check(setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); - } + /* attitude setpoint */ + orb_check(vehicle_attitude_setpoint_sub, &updated); - /* get a local copy of the current sensor values */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + } + /* offboard control setpoint */ + orb_check(offboard_control_setpoint_sub, &updated); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); + } + + /* vehicle status */ + orb_check(vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + } + /* sensors */ + orb_check(sensor_combined_sub, &updated); - } else if (state.flag_control_manual_enabled) { + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + } + + /* set flag to safe value */ + control_yaw_position = true; - if (state.flag_control_attitude_enabled) { + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } + + /* define which input is the dominating control input */ + if (control_mode.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + + } else if (control_mode.flag_control_manual_enabled) { + /* manual input */ + if (control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; + + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = true; } - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; + } else { + /* only move yaw setpoint if manual input is != 0 */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { + /* control yaw rate */ + control_yaw_position = false; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } - } - } + control_yaw_position = true; + } + } + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); } + } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; + } - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + att_sp.timestamp = hrt_absolute_time(); - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + /* manual rate inputs (ACRO), from RC control or joystick */ + if (control_mode.flag_control_rates_enabled) { + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); - - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } } - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } + + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } - float gyro[3]; + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + /* run rates controller if needed */ + if (control_mode.flag_control_rates_enabled) { /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + bool rates_sp_updated = false; + orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; + } - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* fill in manual control values */ + actuators.control[4] = manual.flaps; + actuators.control[5] = manual.aux1; + actuators.control[6] = manual.aux2; + actuators.control[7] = manual.aux3; - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - perf_end(mc_loop_perf); - } /* end of poll call for attitude updates */ - } /* end of poll return value check */ + perf_end(mc_loop_perf); + } } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) @@ -408,10 +389,9 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(att_sub); - close(state_sub); - close(manual_sub); + close(vehicle_attitude_sub); + close(vehicle_control_mode_sub); + close(manual_control_setpoint_sub); close(actuator_pub); close(att_sp_pub); @@ -467,11 +447,11 @@ int multirotor_att_control_main(int argc, char *argv[]) thread_should_exit = false; mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index af5bac88a..8245aa560 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -62,16 +62,16 @@ #include <systemlib/param/param.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_IMAX, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_IMAX, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); //PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); @@ -246,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c47..431a435f7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d5..adb63186c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,14 +59,14 @@ #include <systemlib/err.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86..ca7794c59 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d04847745..bc4b48fb4 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f39d11438..36dd370fb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,13 +35,14 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <math.h> #include <stdbool.h> #include <unistd.h> #include <fcntl.h> @@ -52,15 +53,22 @@ #include <sys/prctl.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/systemlib.h> +#include <systemlib/pid/pid.h> +#include <mavlink/mavlink_log.h> #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) +static float scale_control(float ctl, float end, float dz); + +static float norm(float x, float y); + +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -92,9 +104,9 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to task_spawn(). */ int multirotor_pos_control_main(int argc, char *argv[]) { @@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor pos control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn_cmd("multirotor_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + warnx("app is running"); + } else { - printf("\tmultirotor pos control app not started\n"); + warnx("app not started"); } + exit(0); } @@ -137,98 +153,531 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) +static float scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +static float norm(float x, float y) +{ + return sqrtf(x * x + y * y); +} + +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + warnx("started"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_position_setpoint_s global_pos_sp; + memset(&global_pos_sp, 0, sizeof(global_pos_sp)); + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - /* publish attitude setpoint */ + /* publish setpoint */ + orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); + orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + bool reset_mission_sp = false; + bool global_pos_sp_valid = false; + bool reset_man_sp_z = true; + bool reset_man_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_auto_sp_xy = true; + bool reset_auto_sp_z = true; + bool reset_takeoff_sp = true; + + hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; + uint64_t local_ref_timestamp = 0; + + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; + thread_running = true; - int loopcounter = 0; - - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); - - - while (1) { - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + struct multirotor_position_control_params params; + struct multirotor_position_control_param_handles params_h; + parameters_init(¶ms_h); + parameters_update(¶ms_h, ¶ms); + + + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + + while (!thread_should_exit) { + + bool param_updated; + orb_check(param_sub, ¶m_updated); + + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); + + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; + + if (params.xy_vel_i > 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; + + } else { + i_limit = 0.0f; // not used + } + + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } + + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); + } + + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + reset_mission_sp = true; } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE - - float dT = 1.0f / 50.0f; - - float x_setpoint = 0.0f; - - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + hrt_abstime t = hrt_absolute_time(); + float dt; + + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + + } else { + dt = 0.0f; + } + + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_auto_sp_z = true; + reset_auto_sp_xy = true; + reset_takeoff_sp = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + + t_prev = t; + + if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + + float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + + if (control_mode.flag_control_manual_enabled) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z += local_pos.ref_alt - ref_alt; + } + + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint + } + + /* reset setpoints to current position if needed */ + if (control_mode.flag_control_altitude_enabled) { + if (reset_man_sp_z) { + reset_man_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; + } + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_man_sp_xy) { + reset_man_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ + float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { + /* calculate direction and increment of control in NED frame */ + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * dt; + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ + float pos_vec_x = local_pos_sp.x - local_pos.x; + float pos_vec_y = local_pos_sp.y - local_pos.y; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; + + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } + } + + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ + local_pos_sp.yaw = att_sp.yaw_body; + + /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ + reset_auto_sp_xy = !control_mode.flag_control_position_enabled; + reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; + reset_takeoff_sp = true; + + /* force reprojection of global setpoint after manual mode */ + reset_mission_sp = true; + + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_takeoff_sp) { + reset_takeoff_sp = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); + } + + reset_auto_sp_xy = false; + reset_auto_sp_z = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + reset_mission_sp = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + } + + if (reset_mission_sp) { + reset_mission_sp = false; + /* update global setpoint projection */ + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + att_sp.yaw_body = global_pos_sp.yaw; + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); + + } else { + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + } + + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + } + + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + reset_takeoff_sp = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + reset_mission_sp = true; + } + + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ + local_pos_sp.yaw = att_sp.yaw_body; + + /* reset setpoints after AUTO mode */ + reset_man_sp_xy = true; + reset_man_sp_z = true; + + } else { + /* no control (failsafe), loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); + } + + reset_man_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_man_sp_z) { + reset_man_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); + } + + reset_auto_sp_z = false; + } + + if (control_mode.flag_control_position_enabled) { + if (reset_man_sp_xy) { + reset_man_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + + reset_auto_sp_xy = false; + } + } + + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* run position & altitude controllers, calculate velocity setpoint */ + if (control_mode.flag_control_altitude_enabled) { + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + + } else { + reset_man_sp_z = true; + global_vel_sp.vz = 0.0f; + } + + if (control_mode.flag_control_position_enabled) { + /* calculate velocity set point in NED frame */ + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; + + /* limit horizontal speed */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } + + } else { + reset_man_sp_xy = true; + global_vel_sp.vx = 0.0f; + global_vel_sp.vy = 0.0f; + } + + /* publish new velocity setpoint */ + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); + // TODO subscribe to velocity setpoint if altitude/position control disabled + + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + + if (control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); + att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; + } + + if (control_mode.flag_control_velocity_enabled) { + /* calculate thrust set point in NED frame */ + if (reset_int_xy) { + reset_int_xy = false; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); + } + + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); + + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); + + if (tilt > params.tilt_max) { + tilt = params.tilt_max; + } + + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* position controller disabled, reset setpoints */ + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + reset_mission_sp = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; - } - printf("[multirotor pos control] ending now...\n"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc405..b7041e4d5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,29 +33,80 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c - * - * Parameters for EKF filter + * @file multirotor_pos_control_params.c + * + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { - /* PID parameters */ - h->p = param_find("MC_POS_P"); + h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); + h->z_p = param_find("MPC_Z_P"); + h->z_d = param_find("MPC_Z_D"); + h->z_vel_p = param_find("MPC_Z_VEL_P"); + h->z_vel_i = param_find("MPC_Z_VEL_I"); + h->z_vel_d = param_find("MPC_Z_VEL_D"); + h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->xy_p = param_find("MPC_XY_P"); + h->xy_d = param_find("MPC_XY_D"); + h->xy_vel_p = param_find("MPC_XY_VEL_P"); + h->xy_vel_i = param_find("MPC_XY_VEL_I"); + h->xy_vel_d = param_find("MPC_XY_VEL_D"); + h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); + param_get(h->takeoff_alt, &(p->takeoff_alt)); + param_get(h->takeoff_gap, &(p->takeoff_gap)); + param_get(h->thr_min, &(p->thr_min)); + param_get(h->thr_max, &(p->thr_max)); + param_get(h->z_p, &(p->z_p)); + param_get(h->z_d, &(p->z_d)); + param_get(h->z_vel_p, &(p->z_vel_p)); + param_get(h->z_vel_i, &(p->z_vel_i)); + param_get(h->z_vel_d, &(p->z_vel_d)); + param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->xy_p, &(p->xy_p)); + param_get(h->xy_d, &(p->xy_d)); + param_get(h->xy_vel_p, &(p->xy_vel_p)); + param_get(h->xy_vel_i, &(p->xy_vel_i)); + param_get(h->xy_vel_d, &(p->xy_vel_d)); + param_get(h->xy_vel_max, &(p->xy_vel_max)); + param_get(h->tilt_max, &(p->tilt_max)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a..fc658dadb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,23 +33,59 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include <systemlib/param/param.h> struct multirotor_position_control_params { - float p; - float i; - float d; + float takeoff_alt; + float takeoff_gap; + float thr_min; + float thr_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; + float tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + param_t takeoff_alt; + param_t takeoff_gap; + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_d; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t xy_p; + param_t xy_d; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6..000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier <lm@inf.ethz.ch> -// * @author Laurens Mackay <mackayl@student.ethz.ch> -// * @author Tobias Naegeli <naegelit@student.ethz.ch> -// * @author Martin Rutschmann <rutmarti@student.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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include <stdio.h> -// #include <stdlib.h> -// #include <stdio.h> -// #include <stdint.h> -// #include <math.h> -// #include <stdbool.h> -// #include <float.h> -// #include <systemlib/pid/pid.h> - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 000000000..b985630ae --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, 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 thrust_pid.c + * + * Implementation of thrust control PID. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "thrust_pid.h" +#include <math.h> + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) +{ + /* Alternative integral component calculation + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + /* calculate the error integral */ + i = pid->integral + (pid->ki * error * dt); + + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ + if (output < pid->limit_min || output > pid->limit_max) { + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); + + } else { + if (isfinite(i)) { + pid->integral = i; + } + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h new file mode 100644 index 000000000..5e169c1ba --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, 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 thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include <stdint.h> + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk new file mode 100644 index 000000000..0404b06c7 --- /dev/null +++ b/src/modules/navigator/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. +# +############################################################################ + +# +# Main Navigation Controller +# + +MODULE_COMMAND = navigator + +SRCS = navigator_main.cpp \ + navigator_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp new file mode 100644 index 000000000..f6c44444a --- /dev/null +++ b/src/modules/navigator/navigator_main.cpp @@ -0,0 +1,604 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 navigator_main.c + * Implementation of the main navigation state machine. + * + * Handles missions, geo fencing and failsafe navigation behavior. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/mission.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +/** + * navigator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int navigator_main(int argc, char *argv[]); + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the sensors task. + */ + ~Navigator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + + orb_advert_t _triplet_pub; /**< position setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + struct { + float throttle_cruise; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t throttle_cruise; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for set triplet updates. + */ + void mission_poll(); + + /** + * Control throttle. + */ + float control_throttle(float energy_error); + + /** + * Control pitch. + */ + float control_pitch(float altitude_error); + + void calculate_airspeed_errors(); + void calculate_gndspeed_undershoot(); + void calculate_altitude_error(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace navigator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Navigator *g_navigator; +} + +Navigator::Navigator() : + + _task_should_exit(false), + _navigator_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _triplet_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), +/* states */ + _mission_items_maxcount(20), + _mission_valid(false), + _loiter_hold(false) +{ + _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); + if (!_mission_items) { + _mission_items_maxcount = 0; + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + + _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Navigator::~Navigator() +{ + if (_navigator_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_navigator_task); + break; + } + } while (_navigator_task != -1); + } + + navigator::g_navigator = nullptr; +} + +int +Navigator::parameters_update() +{ + + //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + return OK; +} + +void +Navigator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +Navigator::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } +} + +void +Navigator::mission_poll() +{ + /* check if there is a new setpoint */ + bool mission_updated; + orb_check(_mission_sub, &mission_updated); + + if (mission_updated) { + + struct mission_s mission; + orb_copy(ORB_ID(mission), _mission_sub, &mission); + + // XXX this is not optimal yet, but a first prototype / + // test implementation + + if (mission.count <= _mission_items_maxcount) { + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_valid = true; + + irqrestore(flags); + } else { + warnx("mission larger than storage space"); + } + } +} + +void +Navigator::task_main_trampoline(int argc, char *argv[]) +{ + navigator::g_navigator->task_main(); +} + +void +Navigator::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _mission_sub = orb_subscribe(ORB_ID(mission)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + parameters_update(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + vehicle_attitude_poll(); + + mission_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + // Current waypoint + math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + // Global position + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* AUTONOMOUS FLIGHT */ + + if (1 /* autonomous flight */) { + + /* execute navigation once we have a setpoint */ + if (_mission_valid) { + + // Next waypoint + math::Vector2f prev_wp; + + if (_global_triplet.previous_valid) { + prev_wp.setX(_global_triplet.previous.lat / 1e7f); + prev_wp.setY(_global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid next waypoint, go for heading hold. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(_global_triplet.current.lat / 1e7f); + prev_wp.setY(_global_triplet.current.lon / 1e7f); + + } + + + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + // XXX to be put in its own class + + if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + + } + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else { + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt; + _loiter_hold = true; + } + + //_parameters.loiter_hold_radius + } + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + continue; + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + continue; + } + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + // XXX define launch position and return + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) { + // XXX flared descent on final approach + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + /* apply minimum pitch if altitude has not yet been reached */ + if (_global_pos.alt < _global_triplet.current.altitude) { + _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); + } + } + + /* lazily publish the setpoint only once available */ + if (_triplet_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _navigator_task = -1; + _exit(0); +} + +int +Navigator::start() +{ + ASSERT(_navigator_task == -1); + + /* start the task */ + _navigator_task = task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); + + if (_navigator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int navigator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: navigator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (navigator::g_navigator != nullptr) + errx(1, "already running"); + + navigator::g_navigator = new Navigator; + + if (navigator::g_navigator == nullptr) + errx(1, "alloc failed"); + + if (OK != navigator::g_navigator->start()) { + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (navigator::g_navigator) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 000000000..06df9a452 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 navigator_params.c + * + * Parameters defined by the navigator task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Navigator parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); + diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 000000000..13328edb4 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,31 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +{ + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; + x[i] += ewdt; + + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; + + } else if (i == 1) { + x[2] += w * ewdt; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 000000000..761c17097 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#include <stdbool.h> +#include <drivers/drv_hrt.h> + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk new file mode 100644 index 000000000..939d76849 --- /dev/null +++ b/src/modules/position_estimator_inav/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 position_estimator_inav +# + +MODULE_COMMAND = position_estimator_inav +SRCS = position_estimator_inav_main.c \ + position_estimator_inav_params.c \ + inertial_filter.c diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c new file mode 100644 index 000000000..10007bf96 --- /dev/null +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -0,0 +1,653 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_main.c + * Model-identification based position estimator for multirotors + */ + +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <string.h> +#include <nuttx/config.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/optical_flow.h> +#include <mavlink/mavlink_log.h> +#include <poll.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <systemlib/systemlib.h> +#include <drivers/drv_hrt.h> + +#include "position_estimator_inav_params.h" +#include "inertial_filter.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + +__EXPORT int position_estimator_inav_main(int argc, char *argv[]); + +int position_estimator_inav_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + +/** + * Print the correct usage. + */ +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} + +/** + * The position_estimator_inav_thread 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 position_estimator_inav_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + if (thread_running) { + printf("position_estimator_inav already running\n"); + /* this is not an error */ + exit(0); + } + + verbose_mode = false; + + if (argc > 1) + if (!strcmp(argv[2], "-v")) + verbose_mode = true; + + thread_should_exit = false; + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_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) { + printf("\tposition_estimator_inav is running\n"); + + } else { + printf("\tposition_estimator_inav not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[inav] started"); + + /* initialize values */ + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; + + int baro_init_cnt = 0; + int baro_init_num = 200; + float baro_alt0 = 0.0f; /* to determine while start up */ + float alt_avg = 0.0f; + bool landed = true; + hrt_abstime landed_time = 0; + bool flag_armed = false; + + uint32_t accel_counter = 0; + uint32_t baro_counter = 0; + + /* declare and safely initialize all structs */ + struct actuator_controls_s actuator; + memset(&actuator, 0, sizeof(actuator)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + + /* subscribe */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* advertise */ + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + struct position_estimator_inav_params params; + struct position_estimator_inav_param_handles pos_inav_param_handles; + /* initialize parameter handles */ + parameters_init(&pos_inav_param_handles); + + /* first parameters read at start up */ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ + parameters_update(&pos_inav_param_handles, ¶ms); + + struct pollfd fds_init[1] = { + { .fd = sensor_combined_sub, .events = POLLIN }, + }; + + /* wait for initial baro value */ + bool wait_baro = true; + + thread_running = true; + + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); + + if (ret < 0) { + /* poll error */ + errx(1, "subscriptions poll error on init."); + + } else if (ret > 0) { + if (fds_init[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + + if (wait_baro && sensor.baro_counter > baro_counter) { + baro_counter = sensor.baro_counter; + + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + baro_alt0 += sensor.baro_alt_meter; + baro_init_cnt++; + + } else { + wait_baro = false; + baro_alt0 /= (float) baro_init_cnt; + warnx("init baro: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.z_global = true; + } + } + } + } + } + + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + + hrt_abstime t_prev = 0; + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; + + /* main loop */ + struct pollfd fds[7] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = actuator_sub, .events = POLLIN }, + { .fd = armed_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; + + if (!thread_should_exit) { + warnx("main loop started."); + } + + while (!thread_should_exit) { + int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + + if (ret < 0) { + /* poll error */ + warnx("subscriptions poll error."); + thread_should_exit = true; + continue; + + } else if (ret > 0) { + /* parameter update */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, + &update); + /* update parameters */ + parameters_update(&pos_inav_param_handles, ¶ms); + } + + /* actuator */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); + } + + /* armed */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + } + + /* vehicle attitude */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + } + + /* sensor combined */ + if (fds[4].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + + if (sensor.accelerometer_counter > accel_counter) { + if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; + + /* transform acceleration vector from body frame to NED frame */ + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + + accel_corr[0] = accel_NED[0] - x_est[2]; + accel_corr[1] = accel_NED[1] - y_est[2]; + accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + + } else { + memset(accel_corr, 0, sizeof(accel_corr)); + } + + accel_counter = sensor.accelerometer_counter; + accel_updates++; + } + + if (sensor.baro_counter > baro_counter) { + baro_corr = - sensor.baro_alt_meter - z_est[0]; + baro_counter = sensor.baro_counter; + baro_updates++; + } + } + + /* optical flow */ + if (fds[5].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + + if (fabsf(sonar_corr) > params.sonar_err) { + // correction is too large: spike or new ground level? + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + // spike detected, ignore + sonar_corr = 0.0f; + + } else { + // new ground level + baro_alt0 += sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + z_est[0] += sonar_corr; + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0f; + } + } + } + + } else { + sonar_corr = 0.0f; + } + + flow_updates++; + } + + /* vehicle GPS position */ + if (fds[6].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + /* initialize reference position if needed */ + if (!ref_xy_inited) { + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } + } else { + ref_xy_init_start = 0; + } + } + + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + } + + } else { + /* no GPS lock */ + memset(gps_corr, 0, sizeof(gps_corr)); + ref_xy_init_start = 0; + } + + gps_updates++; + } + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + t_prev = t; + + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ + baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); + + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow + + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; + + if (can_estimate_xy) { + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ + inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } + } + } + + /* detect land */ + alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp = z_est[0] - alt_avg; + alt_disp = alt_disp * alt_disp; + float land_disp2 = params.land_disp * params.land_disp; + /* get actual thrust output */ + float thrust = armed.armed ? actuator.control[3] : 0.0f; + + if (landed) { + if (alt_disp > land_disp2 && thrust > params.land_thr) { + landed = false; + landed_time = 0; + } + + } else { + if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (landed_time == 0) { + landed_time = t; // land detected first time + + } else { + if (t > landed_time + params.land_t * 1000000.0f) { + landed = true; + landed_time = 0; + } + } + + } else { + landed_time = 0; + } + } + + if (verbose_mode) { + /* print updates rate */ + if (t > updates_counter_start + updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt, + flow_updates / updates_dt); + updates_counter_start = t; + accel_updates = 0; + baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; + flow_updates = 0; + } + } + + if (t > pub_last + pub_interval) { + pub_last = t; + /* publish local position */ + local_pos.timestamp = t; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.x = x_est[0]; + local_pos.vx = x_est[1]; + local_pos.y = y_est[0]; + local_pos.vy = y_est[1]; + local_pos.z = z_est[0]; + local_pos.vz = z_est[1]; + local_pos.landed = landed; + local_pos.yaw = att.yaw; + + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + /* publish global position */ + global_pos.valid = local_pos.xy_global; + + if (local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; + } + + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + } + + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; + } + + if (local_pos.z_global) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.yaw = local_pos.yaw; + + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + flag_armed = armed.armed; + } + + warnx("exiting."); + mavlink_log_info(mavlink_fd, "[inav] exiting"); + thread_running = false; + return 0; +} diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c new file mode 100644 index 000000000..4f9ddd009 --- /dev/null +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_params.c + * + * Parameters for position_estimator_inav + */ + +#include "position_estimator_inav_params.h" + +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); +PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); + +int parameters_init(struct position_estimator_inav_param_handles *h) +{ + h->w_alt_baro = param_find("INAV_W_ALT_BARO"); + h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); + h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); + h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); + h->w_pos_acc = param_find("INAV_W_POS_ACC"); + h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); + h->flow_k = param_find("INAV_FLOW_K"); + h->sonar_filt = param_find("INAV_SONAR_FILT"); + h->sonar_err = param_find("INAV_SONAR_ERR"); + h->land_t = param_find("INAV_LAND_T"); + h->land_disp = param_find("INAV_LAND_DISP"); + h->land_thr = param_find("INAV_LAND_THR"); + + return OK; +} + +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ + param_get(h->w_alt_baro, &(p->w_alt_baro)); + param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_alt_sonar, &(p->w_alt_sonar)); + param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); + param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); + param_get(h->w_pos_acc, &(p->w_pos_acc)); + param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_acc_bias, &(p->w_acc_bias)); + param_get(h->flow_k, &(p->flow_k)); + param_get(h->sonar_filt, &(p->sonar_filt)); + param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->land_t, &(p->land_t)); + param_get(h->land_disp, &(p->land_disp)); + param_get(h->land_thr, &(p->land_thr)); + + return OK; +} diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h new file mode 100644 index 000000000..61570aea7 --- /dev/null +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@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 position_estimator_inav_params.h + * + * Parameters for Position Estimator + */ + +#include <systemlib/param/param.h> + +struct position_estimator_inav_params { + float w_alt_baro; + float w_alt_acc; + float w_alt_sonar; + float w_pos_gps_p; + float w_pos_gps_v; + float w_pos_acc; + float w_pos_flow; + float w_acc_bias; + float flow_k; + float sonar_filt; + float sonar_err; + float land_t; + float land_disp; + float land_thr; +}; + +struct position_estimator_inav_param_handles { + param_t w_alt_baro; + param_t w_alt_acc; + param_t w_alt_sonar; + param_t w_pos_gps_p; + param_t w_pos_gps_v; + param_t w_pos_acc; + param_t w_pos_flow; + param_t w_acc_bias; + param_t flow_k; + param_t sonar_filt; + param_t sonar_err; + param_t land_t; + param_t land_disp; + param_t land_thr; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct position_estimator_inav_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd1329..363961819 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index fbd82a4c6..5c621cfb2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -108,9 +108,11 @@ controls_tick() { perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + r_raw_rc_count = 8; + } perf_end(c_gather_sbus); /* @@ -124,8 +126,6 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. @@ -197,7 +197,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + ASSERT(mapped < PX4IO_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ @@ -209,7 +209,7 @@ controls_tick() { } /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } @@ -286,7 +286,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { @@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_CONTROL_CHANNELS) + *num_values = PX4IO_CONTROL_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 745cdfa40..fd3b72015 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -53,13 +53,13 @@ #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ -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*/ +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 @@ -243,6 +243,9 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; @@ -254,13 +257,21 @@ dsm_bind(uint16_t cmd, int pulses) case dsm_bind_power_down: /*power down DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif break; case dsm_bind_power_up: /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif dsm_guess_format(true); break; @@ -274,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, false); up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); } break; @@ -288,6 +299,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } +#endif } /** @@ -382,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (dsm_channel_shift == 11) + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ *num_values |= 0x8000; + } /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. @@ -407,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * 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 + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @return true=decoded raw channel values updated, false=no update */ bool diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..79b6546b3 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -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 @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,12 @@ i2c_init(void) #endif } - /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +331,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..05897b4ce 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -47,6 +47,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <systemlib/mixer/mixer.h> extern "C" { @@ -68,6 +69,8 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static bool should_arm = false; +static bool should_always_enable_pwm = false; /* selected control values and count for mixing */ enum mixer_source { @@ -98,7 +101,7 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { @@ -112,12 +115,11 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -132,7 +134,8 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; @@ -149,12 +152,36 @@ mixer_tick(void) } /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ @@ -164,59 +191,42 @@ mixer_tick(void) } else if (source != MIX_NONE) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); - - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) - ); - - if (should_arm && !mixer_servos_armed) { + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the disarmed servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } @@ -265,9 +275,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while safety off */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { return; } @@ -344,16 +353,17 @@ mixer_set_failsafe() * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) return; /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -364,7 +374,7 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servo_failsafe[i] = 0; } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f4..01869569f 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,17 +3,23 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -
\ No newline at end of file + ../systemlib/pwm_limit/pwm_limit.c + +ifeq ($(BOARD),px4io-v1) +SRCS += i2c.c +endif +ifeq ($(BOARD),px4io-v2) +SRCS += serial.c \ + ../systemlib/hx_stream.c +endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 88d8cc87c..5e5396782 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -45,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - /* Per C, this is safe for all 2's complement systems */ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) @@ -75,10 +74,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PROTOCOL_VERSION 4 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ @@ -93,7 +94,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,19 +106,24 @@ #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_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* 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 */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -143,7 +149,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -152,37 +158,42 @@ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #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) +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#endif -#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 */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo 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 }; + /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -194,10 +205,23 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. @@ -218,3 +242,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..ff9eecd74 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -50,6 +50,7 @@ #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <stm32_uart.h> @@ -64,10 +65,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif +pwm_limit_t pwm_limit; /* * a set of debug buffers to allow us to send debug information from ISRs @@ -147,8 +145,10 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif /* start the safety switch handler */ safety_init(); @@ -159,10 +159,8 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 - /* start the i2c handler */ - i2c_init(); -#endif + /* start the FMU interface */ + interface_init(); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -176,6 +174,9 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { @@ -215,6 +216,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -223,15 +225,15 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 57cffcc23..4fea0288c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,15 +42,18 @@ #include <stdbool.h> #include <stdint.h> -#include <drivers/boards/px4io/px4io_internal.h> +#include <board_config.h> #include "protocol.h" +#include <systemlib/pwm_limit/pwm_limit.h> + /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels /* * Debug logging @@ -77,6 +80,9 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. @@ -96,7 +102,9 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) @@ -117,34 +125,50 @@ struct sys_state_s { extern struct sys_state_s system_state; /* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + +/* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif -#ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - /* * Mixer */ @@ -156,17 +180,16 @@ extern void mixer_handle_text(const void *buffer, size_t length); */ extern void safety_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** @@ -186,15 +209,10 @@ 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); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ +/** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); - -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a922362b6..86a40bc22 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -44,6 +44,7 @@ #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include "px4io.h" #include "protocol.h" @@ -57,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -79,7 +84,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0 }; /** @@ -87,14 +95,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -104,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -114,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -137,8 +145,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, }; @@ -146,8 +160,11 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -166,7 +183,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -182,9 +199,33 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; -void +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX }; + +/** + * PAGE 108 + * + * disarmed PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -233,10 +274,17 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; @@ -247,6 +295,85 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { + r_page_servo_control_min[offset] = *values; + } + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { + r_page_servo_control_max[offset] = *values; + } + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { + r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; + all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } + + offset++; + num_values--; + values++; + } + + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -260,11 +387,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -317,8 +446,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } + + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } r_setup_arming = value; @@ -346,13 +481,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - 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); + POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); + POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); + POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); + POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); + break; +#endif + + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: @@ -371,9 +512,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } @@ -381,7 +525,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -401,6 +545,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; @@ -426,7 +573,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } @@ -446,6 +593,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } @@ -482,6 +637,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -515,7 +671,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -525,26 +682,60 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; + } + } +#endif + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); - +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif + +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -587,6 +778,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); + break; default: return -1; @@ -616,7 +816,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 073ddeaba..c523df6ca 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,7 +66,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values); + return sbus_decode(now, values, num_values, max_channels); } /* @@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..e9adc8cd6 --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,338 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file serial.c + * + * Serial communication for the PX4IO module. + */ + +#include <stdint.h> +#include <unistd.h> +#include <termios.h> +#include <fcntl.h> +#include <string.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> +#include <stm32.h> +#include <systemlib/perf_counter.h> + +//#define DEBUG +#include "px4io.h" + +static perf_counter_t pc_txns; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; +static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; + +static void rx_handle_packet(void); +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + +static struct IOPacket dma_packet; + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +void +interface_init(void) +{ + pc_txns = perf_alloc(PC_ELAPSED, "txns"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); + + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* clear status/errors */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif + + /* configure RX DMA and return to listening state */ + dma_reset(); + + debug("serial init"); +} + +static void +rx_handle_packet(void) +{ + /* check packet CRC */ + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { + perf_count(pc_crcerr); + + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { + + /* it's a blind write - pass it on */ + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { + + /* it's a read - get register pointer for reply */ + unsigned count; + uint16_t *registers; + + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count * 2); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; + } + + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ + perf_begin(pc_txns); + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* handle the received packet */ + rx_handle_packet(); + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + dma_reset(); + + /* send the reply to the just-processed request */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(&dma_packet); + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + PKT_SIZE(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + stm32_dmastart(tx_dma, NULL, NULL, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); +} + +static int +serial_interrupt(int irq, void *context) +{ + static bool abort_on_idle = false; + + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; + + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; + } + + if (sr & USART_SR_IDLE) { + + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ + perf_count(pc_badidle); + dma_reset(); + return 0; + } + + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ + perf_count(pc_idle); + stm32_dmastop(rx_dma); + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} + diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ba7cdd91c..f94875d5b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include <systemlib/err.h> #include <unistd.h> #include <drivers/drv_hrt.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -75,6 +76,7 @@ #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/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -83,18 +85,19 @@ #include <uORB/topics/esc_status.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" +#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -102,9 +105,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -183,8 +183,17 @@ static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -static void write_formats(int fd); +static int write_formats(int fd); +/** + * Write version message to log file. + */ +static int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -233,24 +242,24 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -262,7 +271,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -357,10 +366,13 @@ static void *logwriter_thread(void *arg) struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_file = open_logfile(); + int log_fd = open_logfile(); - /* write log messages formats */ - write_formats(log_file); + /* write log messages formats, version and parameters */ + log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; @@ -387,11 +399,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -404,12 +411,9 @@ static void *logwriter_thread(void *arg) n = available; } - n = write(log_file, read_ptr, n); + n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -422,31 +426,23 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif + /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } + should_wait = true; } if (++poll_count == 10) { - fsync(log_file); + fsync(log_fd); poll_count = 0; } } - fsync(log_file); - close(log_file); - -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif + fsync(log_fd); + close(log_fd); return OK; } @@ -500,34 +496,92 @@ void sdlog2_stop_log() /* wait for write thread to return */ int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } + logwriter_pthread = 0; sdlog2_status(); } - -void write_formats(int fd) +int write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; - } log_format_packet = { + } log_msg_format = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; - /* fill message format packet for each format and write to log */ - int i; + int written = 0; + + /* fill message format packet for each format and write it */ + for (int i = 0; i < log_formats_num; i++) { + log_msg_format.body = log_formats[i]; + written += write(fd, &log_msg_format, sizeof(log_msg_format)); + } + + return written; +} + +int write_version(int fd) +{ + /* construct version message */ + struct { + LOG_PACKET_HEADER; + struct log_VER_s body; + } log_msg_VER = { + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), + }; + + /* fill version message and write it */ + strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); + return write(fd, &log_msg_VER, sizeof(log_msg_VER)); +} + +int write_parameters(int fd) +{ + /* construct parameter message */ + struct { + LOG_PACKET_HEADER; + struct log_PARM_s body; + } log_msg_PARM = { + LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), + }; + + int written = 0; + param_t params_cnt = param_count(); + + for (param_t param = 0; param < params_cnt; param++) { + /* fill parameter message and write it */ + strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); + float value = NAN; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + param_get(param, &i); + value = i; // cast integer to float + break; + } + + case PARAM_TYPE_FLOAT: + param_get(param, &value); + break; - for (i = 0; i < log_formats_num; i++) { - log_format_packet.body = log_formats[i]; - log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + default: + break; + } + + log_msg_PARM.body.value = value; + written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); } - fsync(fd); + return written; } int sdlog2_thread_main(int argc, char *argv[]) @@ -605,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[]) } const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(150); + char *converter_out = malloc(120); 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 */ @@ -623,15 +678,8 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 19; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ @@ -655,7 +703,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; + memset(&buf, 0, sizeof(buf)); struct { @@ -678,6 +728,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -703,6 +754,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -710,6 +762,14 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; @@ -816,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; - fdsc_count++; + fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); @@ -824,6 +884,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -898,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } - ifds = 1; // Begin from fds[1] again + ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -911,15 +977,14 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } @@ -1065,10 +1130,12 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } @@ -1156,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - for (uint8_t i=0; i<buf.esc.esc_count; i++) - { + + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; log_msg.body.log_ESC.esc_count = buf.esc.esc_count; @@ -1175,14 +1242,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } @@ -1202,6 +1273,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); + free(lb.data); + warnx("exiting."); thread_running = false; @@ -1297,8 +1370,11 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + // TODO use flag from actuator_armed here? + bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + + if (armed != flag_system_armed) { + flag_system_armed = armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 5c175ef7e..dc5e6c8bd 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -85,10 +85,10 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } #define LOG_FORMAT_MSG 0x80 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 934e4dec8..90093a407 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -48,12 +48,6 @@ /* define message formats */ #pragma pack(push, 1) -/* --- TIME - TIME STAMP --- */ -#define LOG_TIME_MSG 1 -struct log_TIME_s { - uint64_t t; -}; - /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { @@ -106,10 +100,12 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; + uint8_t xy_flags; + uint8_t z_flags; + uint8_t landed; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -149,15 +145,14 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - uint8_t state; - uint8_t flight_mode; - uint8_t manual_control_mode; - uint8_t manual_sas_mode; - uint8_t armed; + uint8_t main_state; + uint8_t navigation_state; + uint8_t arming_state; float battery_voltage; float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -244,21 +239,48 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; + +/* --- VER - VERSION --- */ +#define LOG_VER_MSG 130 +struct log_VER_s { + char arch[16]; + char fw_git[64]; +}; + +/* --- PARM - PARAMETER --- */ +#define LOG_PARM_MSG 131 +struct log_PARM_s { + char name[16]; + float value; +}; + #pragma pack(pop) /* construct list of all message formats */ - static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "StartTime"), + /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), @@ -266,7 +288,13 @@ static const struct log_format_s log_formats[] = { 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"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + /* system-level messages, ID >= 0x80 */ + // FMT: don't write format of format message, it's useless + LOG_FORMAT(TIME, "Q", "StartTime"), + LOG_FORMAT(VER, "NZ", "Arch,FwGit"), + LOG_FORMAT(PARM, "Nf", "Name,Value"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h new file mode 100644 index 000000000..c6a9ba638 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, 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 sdlog2_version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef SDLOG2_VERSION_H_ +#define SDLOG2_VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* SDLOG2_VERSION_H_ */ diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445..96a443c6e 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { 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) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _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) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bd431c9eb..587b81588 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include <systemlib/param/param.h> +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); @@ -68,7 +94,11 @@ 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_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -154,34 +184,45 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); 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_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 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 */ +#endif +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* 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)); +/* PX4IOAR: 0.00838095238 */ +/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ +/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ -PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b971..da0c11372 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include <stdio.h> #include <errno.h> #include <math.h> +#include <mathlib/mathlib.h> #include <nuttx/analog/adc.h> @@ -66,15 +67,16 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <conversion/rotation.h> -#include <systemlib/ppm_decode.h> #include <systemlib/airspeed.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -104,22 +106,22 @@ * 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 +#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 +#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 @@ -133,8 +135,6 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ - #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** @@ -167,12 +167,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ - hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ + hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ /** - * Gather and publish PPM input data. + * Gather and publish RC input data. */ - void ppm_poll(); + void rc_poll(); /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -189,14 +189,15 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -210,13 +211,16 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -226,21 +230,22 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; - int rc_type; + int board_rotation; + int external_mag_rotation; int rc_map_roll; int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_assisted_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -265,10 +270,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -277,19 +278,19 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_assisted_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -306,6 +307,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -380,9 +384,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -421,7 +425,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _ppm_last_valid(0), + _rc_last_valid(0), _fd_adc(-1), _last_adc(0), @@ -437,20 +441,25 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), /* publications */ _sensor_pub(-1), _manual_control_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3, 3), + _external_mag_rotation(3, 3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -479,8 +488,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -488,16 +495,16 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -537,9 +544,14 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -573,6 +585,8 @@ int Sensors::parameters_update() { bool rc_valid = true; + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -583,29 +597,27 @@ Sensors::parameters_update() 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]); + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f)) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; - } + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -623,54 +635,35 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); - } - - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); } - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); 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)); @@ -682,15 +675,14 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -725,12 +717,19 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } @@ -749,7 +748,7 @@ Sensors::accel_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the accel internal sampling rate up to at leat 1000Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 1000); @@ -757,7 +756,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -765,7 +764,10 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); - #endif +#else +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + +#endif warnx("using system accel"); close(fd); @@ -787,7 +789,7 @@ Sensors::gyro_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) @@ -797,15 +799,15 @@ Sensors::gyro_init() if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else +#else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); - #endif +#endif warnx("using system gyro"); close(fd); @@ -816,6 +818,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -824,11 +827,38 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); + + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } + + + + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; close(fd); } @@ -842,7 +872,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ @@ -874,9 +904,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation * vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +930,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation * vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +956,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + + if (_mag_is_external) + vect = _external_mag_rotation * vect; + else + vect = _board_rotation * vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; @@ -963,8 +1006,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -977,21 +1020,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1093,6 +1136,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { @@ -1102,12 +1148,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); } /* look for specific channels and process the raw voltage to measurement data */ @@ -1135,18 +1181,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else { _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); } - } + } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor @@ -1171,22 +1218,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } void -Sensors::ppm_poll() +Sensors::rc_poll() { + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - 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 (poll_ret > 0) { + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); struct manual_control_setpoint_s manual_control; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1194,10 +1238,11 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1216,7 +1261,7 @@ Sensors::ppm_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _ppm_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1226,6 +1271,7 @@ Sensors::ppm_poll() */ if (rc_input.values[i] < _parameters.min[i]) rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) rc_input.values[i] = _parameters.max[i]; @@ -1297,11 +1343,17 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1313,21 +1365,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); - } - - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { @@ -1350,6 +1398,16 @@ Sensors::ppm_poll() manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); } + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + /* check if ready for publishing */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1366,6 +1424,14 @@ Sensors::ppm_poll() } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } + + /* check if ready for publishing */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } @@ -1381,8 +1447,7 @@ Sensors::task_main() { /* inform about start */ - printf("[sensors] Initializing..\n"); - fflush(stdout); + warnx("Initializing.."); /* start individual sensors */ accel_init(); @@ -1400,12 +1465,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ orb_set_interval(_gyro_sub, 4); @@ -1461,7 +1526,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); @@ -1485,7 +1550,7 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); /* Look for new r/c input data */ - ppm_poll(); + rc_poll(); perf_end(_loop_perf); } @@ -1503,11 +1568,11 @@ Sensors::start() /* start the task */ _sensors_task = task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Sensors::task_main_trampoline, + nullptr); if (_sensors_task < 0) { warn("task start failed"); diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index e01cc4dda..310fbf60f 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -42,7 +42,7 @@ #include <stdio.h> #include <math.h> -#include "conversions.h" +#include <geo/geo.h> #include "airspeed.h" @@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; -// printf("[airspeed] Invalid air density, using density at sea level\n"); + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; } float pressure_difference = total_pressure - static_pressure; - if(pressure_difference > 0) { + if (pressure_difference > 0) { return sqrtf((2.0f*(pressure_difference)) / density); - } else - { + } else { return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index def53f0c1..8dccaab9c 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -85,6 +85,14 @@ */ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + /** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index ac94252c5..9105d83cb 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - -void rot2quat(const float R[9], float Q[4]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - int32_t idx; - - /* conversion of rotation matrix to quaternion - * choose the largest component to begin with */ - q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; - q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; - q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; - q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; - - idx = 0; - - if (q0_2 < q1_2) { - q0_2 = q1_2; - - idx = 1; - } - - if (q0_2 < q2_2) { - q0_2 = q2_2; - idx = 2; - } - - if (q0_2 < q3_2) { - q0_2 = q3_2; - idx = 3; - } - - q0_2 = sqrtf(q0_2); - - /* solve for the remaining three components */ - if (idx == 0) { - q1_2 = q0_2; - q2_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[6] - R[2]) / 4.0F / q0_2; - q0_2 = (R[1] - R[3]) / 4.0F / q0_2; - - } else if (idx == 1) { - q2_2 = q0_2; - q1_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[6] + R[2]) / 4.0F / q0_2; - - } else if (idx == 2) { - q3_2 = q0_2; - q1_2 = (R[6] - R[2]) / 4.0F / q0_2; - q2_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[7] + R[5]) / 4.0F / q0_2; - - } else { - q1_2 = (R[1] - R[3]) / 4.0F / q0_2; - q2_2 = (R[6] + R[2]) / 4.0F / q0_2; - q3_2 = (R[7] + R[5]) / 4.0F / q0_2; - } - - /* return values */ - Q[0] = q1_2; - Q[1] = q2_2; - Q[2] = q3_2; - Q[3] = q0_2; -} - -void quat2rot(const float Q[4], float R[9]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - - memset(&R[0], 0, 9U * sizeof(float)); - - q0_2 = Q[0] * Q[0]; - q1_2 = Q[1] * Q[1]; - q2_2 = Q[2] * Q[2]; - q3_2 = Q[3] * Q[3]; - - R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; - R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); - R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); - R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); - R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; - R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); - R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); - R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); - R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -} - -float get_air_density(float static_pressure, float temperature_celsius) -{ - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 064426f21..dc383e770 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,7 +43,6 @@ #define CONVERSIONS_H_ #include <float.h> #include <stdint.h> -#include <systemlib/geo/geo.h> __BEGIN_DECLS @@ -57,34 +56,6 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); -/** - * Converts a 3 x 3 rotation matrix to an unit quaternion. - * - * All orientations are expressed in NED frame. - * - * @param R rotation matrix to convert - * @param Q quaternion to write back to - */ -__EXPORT void rot2quat(const float R[9], float Q[4]); - -/** - * Converts an unit quaternion to a 3 x 3 rotation matrix. - * - * All orientations are expressed in NED frame. - * - * @param Q quaternion to convert - * @param R rotation matrix to write back to - */ -__EXPORT void quat2rot(const float Q[4], float R[9]); - -/** - * Calculates air density. - * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius - */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); - __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 192195856..f321f9ceb 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -46,28 +46,33 @@ #include <mavlink/mavlink_log.h> -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +__EXPORT 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"); + lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage)); } -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); +} + +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; } -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { return lb->count == 0; } -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { int end = (lb->start + lb->count) % lb->size; memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); @@ -80,19 +85,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ } } -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { if (!mavlink_logbuffer_is_empty(lb)) { 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 { @@ -100,7 +99,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } } -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { va_list ap; va_start(ap, fmt); diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index df0dfe838..cce46bf5f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <ctype.h> +#include <systemlib/err.h> #include "mixer.h" @@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : @@ -142,6 +171,22 @@ NullMixer * NullMixer::from_text(const char *buf, unsigned &buflen) { NullMixer *nm = nullptr; + const char *end = buf + buflen; + + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + return nm; + } + + } if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { nm = new NullMixer; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9..1c889a811 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,9 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value +#include <nuttx/config.h> #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to @@ -210,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -239,6 +259,11 @@ public: void reset(); /** + * Count the mixers in the group. + */ + unsigned count(); + + /** * Adds mixers to the group based on a text description in a buffer. * * Mixer definitions begin with a single capital letter and a colon. @@ -424,6 +449,7 @@ public: HEX_PLUS, /**< hex in + configuration */ OCTA_X, OCTA_PLUS, + OCTA_COX, MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cd..3ed99fba0 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 000000000..a55ddf8a3 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 mixer_load.c + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> +#include <string.h> +#include <stdio.h> +#include <ctype.h> + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) { + return 1; + } + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* compact whitespace in the buffer */ + char *t, *f; + for (f = line; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } + + /* add the line to the buffer */ + strcat(buf, line); + } + + return 0; +} + diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h new file mode 100644 index 000000000..4b7091d5b --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 mixer_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include <nuttx/config.h> + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8ded0b05c..bf77795d5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,7 +130,17 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const MultirotorMixer::Rotor _config_octa_cox[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { 0.707107, -0.707107, 1.00 }, + { -0.707107, -0.707107, -1.00 }, + { 0.707107, 0.707107, 1.00 }, + { -0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, 1.00 }, + { 0.707107, -0.707107, -1.00 }, +}; +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], &_config_quad_v[0], @@ -139,8 +149,9 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME &_config_hex_plus[0], &_config_octa_x[0], &_config_octa_plus[0], + &_config_octa_cox[0], }; -const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ 4, /* quad_v */ @@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { 6, /* hex_plus */ 8, /* octa_x */ 8, /* octa_plus */ + 8, /* octa_cox */ }; } @@ -181,6 +193,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; + const char *end = buf + buflen; + + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + return nullptr; + } + + } if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { debug("multirotor parse failed on '%s'", buf); @@ -188,11 +217,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); return nullptr; } - buflen -= used; + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; @@ -217,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8x")) { geometry = MultirotorMixer::OCTA_X; + + } else if (!strcmp(geomname, "8c")) { + geometry = MultirotorMixer::OCTA_COX; } else { debug("unrecognised geometry '%s'", geomname); diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 07dc5f37f..c3985b5de 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -71,44 +71,30 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { int ret; int s[5]; + int n = -1; buf = findtag(buf, buflen, 'O'); - if ((buf == nullptr) || (buflen < 12)) + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); return -1; + } - if ((ret = sscanf(buf, "O: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { - debug("scaler parse failed on '%s' (got %d)", buf, ret); + if ((ret = sscanf(buf, "O: %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); + return -1; + } + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); return -1; } - skipline(buf, buflen); scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -126,15 +112,22 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale int s[5]; buf = findtag(buf, buflen, 'S'); - if ((buf == nullptr) || (buflen < 16)) + if ((buf == nullptr) || (buflen < 16)) { + debug("control parser failed finding tag, ret: '%s'", buf); return -1; + } if (sscanf(buf, "S: %u %u %d %d %d %d %d", &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -162,7 +155,11 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c goto out; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); @@ -173,22 +170,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c mixinfo->control_count = inputs; - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); goto out; + } for (unsigned i = 0; i < inputs; i++) { if (parse_control_scaler(end - buflen, buflen, mixinfo->controls[i].scaler, mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); goto out; + } + } sm = new SimpleMixer(control_cb, cb_handle, mixinfo); if (sm != nullptr) { mixinfo = nullptr; - debug("loaded mixer with %d inputs", inputs); + debug("loaded mixer with %d input(s)", inputs); } else { debug("could not allocate memory for mixer"); diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c50..fc7485e20 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -39,4 +39,5 @@ LIBNAME = mixerlib SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ - mixer_simple.cpp + mixer_simple.cpp \ + mixer_load.c diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 683c63040..050bf2f47 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -74,7 +74,18 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus} +set octa_cox { + 45 CCW + -45 CW + -135 CCW + 135 CW + -45 CCW + 45 CW + 135 CCW + -135 CW +} + +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c1227..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -45,7 +45,8 @@ SRCS = err.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ - geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 69a9bdf9b..398657dd7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include <unistd.h> #include <systemlib/err.h> #include <errno.h> +#include <semaphore.h> #include <sys/stat.h> @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_sem = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ @@ -505,27 +508,63 @@ param_get_default_file(void) int param_save_default(void) { - /* delete the file in case it exists */ - unlink(param_get_default_file()); + int res; + int fd; - /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + const char *filename = param_get_default_file(); + const char *filename_tmp = malloc(strlen(filename) + 5); + sprintf(filename_tmp, "%s.tmp", filename); - if (fd < 0) { - warn("opening '%s' for writing failed", param_get_default_file()); - return -1; + /* delete temp file if exist */ + res = unlink(filename_tmp); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete temp file: %s", filename_tmp); + + if (res == OK) { + /* write parameters to temp file */ + fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("failed to open temp file: %s", filename_tmp); + res = ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) + warnx("failed to write parameters to file: %s", filename_tmp); + } + + close(fd); } - int result = param_export(fd, false); - close(fd); + if (res == OK) { + /* delete parameters file */ + res = unlink(filename); - if (result != 0) { - warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); - return -2; + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete parameters file: %s", filename); } - return 0; + if (res == OK) { + /* rename temp file to parameters */ + res = rename(filename_tmp, filename); + + if (res != OK) + warn("failed to rename %s to %s", filename_tmp, filename); + } + + free(filename_tmp); + + return res; } /** diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287..bf84b7945 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a1..e1e3cbe95 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f66..77c952f52 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,6 +51,8 @@ #include "pid.h" #include <math.h> +#define SIGMA 0.000001f + __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min) { @@ -165,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; - } else { - if (!isfinite(i)) { - i = 0.0f; + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } @@ -186,11 +194,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit > SIGMA) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c new file mode 100644 index 000000000..cac3dc82a --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.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 pwm_limit.c + * + * Lib to limit PWM output + * + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include "pwm_limit.h" +#include <math.h> +#include <stdbool.h> +#include <drivers/drv_hrt.h> + +void pwm_limit_init(pwm_limit_t *limit) +{ + limit->state = LIMIT_STATE_OFF; + limit->time_armed = 0; + return; +} + +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +{ + /* first evaluate state changes */ + switch (limit->state) { + case LIMIT_STATE_OFF: + if (armed) + limit->state = LIMIT_STATE_RAMP; + limit->time_armed = hrt_absolute_time(); + break; + case LIMIT_STATE_INIT: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) + limit->state = LIMIT_STATE_RAMP; + break; + case LIMIT_STATE_RAMP: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) + limit->state = LIMIT_STATE_ON; + break; + case LIMIT_STATE_ON: + if (!armed) + limit->state = LIMIT_STATE_OFF; + break; + default: + break; + } + + unsigned progress; + uint16_t temp_pwm; + + /* then set effective_pwm based on state */ + switch (limit->state) { + case LIMIT_STATE_OFF: + case LIMIT_STATE_INIT: + for (unsigned i=0; i<num_channels; i++) { + effective_pwm[i] = disarmed_pwm[i]; + output[i] = 0.0f; + } + break; + case LIMIT_STATE_RAMP: + + progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; + for (unsigned i=0; i<num_channels; i++) { + + uint16_t ramp_min_pwm; + + /* if a disarmed pwm value was set, blend between disarmed and min */ + if (disarmed_pwm[i] > 0) { + + /* safeguard against overflows */ + uint16_t disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) + disarmed = min_pwm[i]; + + uint16_t disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + output[i] = (float)progress/10000.0f * output[i]; + } + break; + case LIMIT_STATE_ON: + for (unsigned i=0; i<num_channels; i++) { + effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + /* effective_output stays the same */ + } + break; + default: + break; + } + return; +} diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h new file mode 100644 index 000000000..9974770be --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.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 pwm_limit.h + * + * Lib to limit PWM output + * + * @author Julian Oes <joes@student.ethz.ch> + */ + +#ifndef PWM_LIMIT_H_ +#define PWM_LIMIT_H_ + +#include <stdint.h> +#include <stdbool.h> + +/* + * time for the ESCs to initialize + * (this is not actually needed if PWM is sent right after boot) + */ +#define INIT_TIME_US 500000 +/* + * time to slowly ramp up the ESCs + */ +#define RAMP_TIME_US 2500000 + +typedef struct { + enum { + LIMIT_STATE_OFF = 0, + LIMIT_STATE_INIT, + LIMIT_STATE_RAMP, + LIMIT_STATE_ON + } state; + uint64_t time_armed; +} pwm_limit_t; + +__BEGIN_DECLS + +__EXPORT void pwm_limit_init(pwm_limit_t *limit); + +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); + +__END_DECLS + +#endif /* PWM_LIMIT_H_ */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..b4350cc24 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 rc_check.c + * + * RC calibration check + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <fcntl.h> + +#include <systemlib/rc_check.h> +#include <systemlib/param/param.h> +#include <mavlink/mavlink_log.h> +#include <uORB/topics/rc_channels.h> + +int rc_calibration_check(int mavlink_fd) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + int channel_fail_count = 0; + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + + channel_fail_count += count; + } + + return channel_fail_count; +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e70b83cce --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(int mavlink_fd); + +__END_DECLS diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 3283aad8a..57a751e1c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -43,13 +43,29 @@ #include <fcntl.h> #include <sched.h> #include <signal.h> -#include <sys/stat.h> #include <unistd.h> #include <float.h> #include <string.h> +#include <sys/stat.h> +#include <sys/types.h> + +#include <stm32_pwr.h> + #include "systemlib.h" +void +systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } + up_systemreset(); +} + static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..3728f2067 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include <float.h> #include <stdint.h> -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +__EXPORT void systemreset(bool to_bootloader) noreturn_function; + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f..000000000 --- a/src/modules/test/foo.c +++ /dev/null @@ -1,4 +0,0 @@ -int test_main(void) -{ - return 0; -}
\ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk deleted file mode 100644 index abf80eedf..000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 301cfa255..3514dca24 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.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 @@ -81,9 +81,15 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); @@ -102,6 +108,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); @@ -114,6 +123,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/vehicle_global_velocity_setpoint.h" +ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); + #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); @@ -123,6 +135,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); @@ -150,6 +165,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); + +#include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h new file mode 100644 index 000000000..6e944ffee --- /dev/null +++ b/src/modules/uORB/topics/actuator_armed.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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 actuator_armed.h + * + * Actuator armed topic + * + */ + +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H + +#include <stdint.h> +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { + + uint64_t timestamp; + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_armed); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index a27095be5..e768ab2f6 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + /** * @addtogroup topics * @{ @@ -72,16 +75,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 30895ca83..446140423 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -60,7 +60,7 @@ 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 */ + unsigned noutputs; /**< valid outputs */ }; /** diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764..e4d2c92ce 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 261a8a4ad..eac6d6e98 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,18 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index e69335b3d..5a8580143 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -48,9 +48,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -63,18 +66,17 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 000000000..a5d21cd4a --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 safety.h + * + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include <stdint.h> +#include "../uORB.h" + +struct safety_s { + + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; + +ORB_DECLARE(safety); + +#endif
\ No newline at end of file diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/uORB/topics/servorail_status.h index 2144ebc34..55668790b 100644 --- a/src/modules/multirotor_pos_control/position_control.h +++ b/src/modules/uORB/topics/servorail_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> + * 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 @@ -36,15 +32,36 @@ ****************************************************************************/ /** - * @file multirotor_position_control.h - * Definition of the position control for a multirotor VTOL + * @file servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ */ -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); -// #endif /* POSITION_CONTROL_H_ */ +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a8..1a245132a 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,8 +64,16 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 31ff014de..e6e4743c6 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -86,7 +86,8 @@ enum VEHICLE_CMD VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_ENUM_END=401, /* | */ + VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END=501, /* | */ }; /** @@ -119,7 +120,7 @@ struct vehicle_command_s float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ + enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 000000000..6184284a4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 000000000..38fb74c9b --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.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 vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_armed; + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + + bool flag_control_auto_enabled; // TEMP + uint8_t auto_state; // TEMP navigation state for AUTO modes +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_mode); + +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c7223..143734e37 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,18 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + bool valid; /**< true if position satisfies validity criteria of estimator */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d..a56434d3b 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h new file mode 100644 index 000000000..73961cdfe --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_global_velocity_setpoint.h + * Definition of the global velocity setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_global_velocity_setpoint_s +{ + float vx; /**< in m/s NED */ + float vy; /**< in m/s NED */ + float vz; /**< in m/s NED */ +}; /**< Velocity setpoint in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_velocity_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 0a7fb4e9d..1639a08c2 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -56,9 +56,9 @@ struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + int32_t lat; /**< Latitude in 1E-7 degrees */ + int32_t lon; /**< Longitude in 1E-7 degrees */ + int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd..427153782 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,29 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ + bool landed; /**< true if vehicle is landed */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 94068a9ac..1c184d3a7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,22 +54,67 @@ #include <stdbool.h> #include "../uORB.h" -/* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, - -} commander_state_machine_t; +/** + * @addtogroup topics @{ + */ + +/* main state machine */ +typedef enum { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; + +/* navigation state machine */ +typedef enum { + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; + +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; + +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -82,26 +127,6 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -128,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -149,55 +174,54 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ + arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ + int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - - bool flag_system_armed; /**< true is motors / actuators are armed */ - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; - bool flag_preflight_airspeed_calibration; + bool is_rotary_wing; + + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; + mission_switch_pos_t mission_switch; + + bool condition_battery_voltage_valid; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; + bool condition_local_altitude_valid; + bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - float load; - float voltage_battery; - float current_battery; + + float load; /**< processor load from 0 to 1 */ + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; @@ -205,15 +229,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ - bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ }; /** diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 7abbf42ae..149b8f6d2 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp) } else { SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) + if (sd != nullptr) { + hrt_cancel(&sd->update_call); delete sd; + } } return CDev::close(filp); diff --git a/src/modules/unit_test/module.mk b/src/modules/unit_test/module.mk new file mode 100644 index 000000000..f00b0f592 --- /dev/null +++ b/src/modules/unit_test/module.mk @@ -0,0 +1,39 @@ +############################################################################ +# +# 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 unit test library. +# + +SRCS = unit_test.cpp + diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp new file mode 100644 index 000000000..64ee544a2 --- /dev/null +++ b/src/modules/unit_test/unit_test.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 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 unit_test.cpp + * A unit test library. + * + */ + +#include "unit_test.h" + +#include <systemlib/err.h> + + +UnitTest::UnitTest() +{ +} + +UnitTest::~UnitTest() +{ +} + +void +UnitTest::print_results(const char* result) +{ + if (result != 0) { + warnx("Failed: %s:%d", mu_last_test(), mu_line()); + warnx("%s", result); + } else { + warnx("ALL TESTS PASSED"); + warnx(" Tests run : %d", mu_tests_run()); + warnx(" Assertion : %d", mu_assertion()); + } +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h new file mode 100644 index 000000000..3020734f4 --- /dev/null +++ b/src/modules/unit_test/unit_test.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 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 unit_test.h + * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). + * + */ + +#ifndef UNIT_TEST_H_ +#define UNIT_TEST_ + +#include <systemlib/err.h> + + +class __EXPORT UnitTest +{ + +public: +#define xstr(s) str(s) +#define str(s) #s +#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } + +INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_assertion) +INLINE_GLOBAL(int, mu_line) +INLINE_GLOBAL(const char*, mu_last_test) + +#define mu_assert(message, test) \ + do \ + { \ + if (!(test)) \ + return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ + else \ + mu_assertion()++; \ + } while (0) + + +#define mu_run_test(test) \ +do \ +{ \ + const char *message; \ + mu_last_test() = #test; \ + mu_line() = __LINE__; \ + message = test(); \ + mu_tests_run()++; \ + if (message) \ + return message; \ +} while (0) + + +public: + UnitTest(); + virtual ~UnitTest(); + + virtual const char* run_tests() = 0; + virtual void print_results(const char* result); +}; + + + +#endif /* UNIT_TEST_H_ */ diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 2dad2261b..80689f20c 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -2,6 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> + * Author: Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +36,7 @@ /** * @file config.c * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> * * config tool. */ @@ -69,27 +71,15 @@ 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."); - } + do_gyro(argc - 2, argv + 2); } if (!strcmp(argv[1], "accel")) { - if (argc >= 3) { - do_accel(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_accel(argc - 2, argv + 2); } if (!strcmp(argv[1], "mag")) { - if (argc >= 3) { - do_mag(argc - 2, argv + 2); - } else { - errx(1, "not enough parameters."); - } + do_mag(argc - 2, argv + 2); } } @@ -100,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -109,28 +100,45 @@ do_gyro(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { - char* end; - int i = strtol(argv[1],&end,10); + /* set the gyro internal sampling rate up to at least i Hz */ + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - if (!strcmp(argv[0], "sampling")) { + if (ret) + errx(ret,"sampling rate could not be set"); - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, i); + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - } else if (!strcmp(argv[0], "rate")) { + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + if (ret) + errx(ret,"pollrate could not be set"); - /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, i); + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i dps */ + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + 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 if (!(argc > 0 && !strcmp(argv[0], "info"))) { - 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"); + } else { + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\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); @@ -148,6 +156,67 @@ do_gyro(int argc, char *argv[]) static void do_mag(int argc, char *argv[]) { + int fd; + int ret; + + fd = open(MAG_DEVICE_PATH, 0); + + if (fd < 0) { + warn("%s", MAG_DEVICE_PATH); + errx(1, "FATAL: no magnetometer found"); + + } else { + + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the mag internal sampling rate up to at least i Hz */ + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i G */ + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + 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 { + errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); + } + + int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); + int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); + int range = ioctl(fd, MAGIOCGRANGE, 0); + + warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); + + close(fd); + } + exit(0); } @@ -155,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -164,34 +234,52 @@ do_accel(int argc, char *argv[]) } else { - if (argc >= 2) { + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the accel internal sampling rate up to at least i Hz */ + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); - char* end; - int i = strtol(argv[1],&end,10); + if (ret) + errx(ret,"sampling rate could not be set"); - if (!strcmp(argv[0], "sampling")) { + } else if (argc == 2 && !strcmp(argv[0], "rate")) { - /* set the accel internal sampling rate up to at leat i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, i); + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); - } else if (!strcmp(argv[0], "rate")) { + if (ret) + errx(ret,"pollrate could not be set"); - /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, i); - } else if (!strcmp(argv[0], "range")) { + } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the range to i dps */ - ioctl(fd, ACCELIOCSRANGE, i); + /* set the range to i G */ + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + 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 if (!(argc > 0 && !strcmp(argv[0], "info"))) { - 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"); + + } else { + errx(1,"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 4' to set measurement range to 4 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); + warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); close(fd); } diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c index 49da51358..2aed80e01 100644 --- a/src/systemcmds/eeprom/eeprom.c +++ b/src/systemcmds/eeprom/eeprom.c @@ -55,7 +55,7 @@ #include <nuttx/fs/nxffs.h> #include <nuttx/fs/ioctl.h> -#include <arch/board/board.h> +#include <board_config.h> #include "systemlib/systemlib.h" #include "systemlib/param/param.h" diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c new file mode 100644 index 000000000..b237b31be --- /dev/null +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.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 esc_calib.c + * + * Tool for ESC calibration + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <poll.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.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/err.h" +#include "drivers/drv_pwm_output.h" + +#include <uORB/topics/actuator_controls.h> + +static void usage(const char *reason); +__EXPORT int esc_calib_main(int argc, char *argv[]); + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + + errx(1, + "usage:\n" + "esc_calib\n" + " [-d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " [-l <pwm> Low PWM value in us (default: %dus)\n" + " [-h <pwm> High PWM value in us (default: %dus)\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Use all outputs\n" + , PWM_DEFAULT_MIN, PWM_DEFAULT_MAX); +} + +int +esc_calib_main(int argc, char *argv[]) +{ + char *dev = PWM_OUTPUT_DEVICE_PATH; + char *ep; + int ch; + int ret; + char c; + + unsigned max_channels = 0; + + uint32_t set_mask = 0; + unsigned long channels; + unsigned single_ch = 0; + + uint16_t pwm_high = PWM_DEFAULT_MAX; + uint16_t pwm_low = PWM_DEFAULT_MIN; + + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + if (argc < 2) { + usage("no channels provided"); + } + + int arg_consumed = 0; + + while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) { + switch (ch) { + + case 'd': + dev = optarg; + arg_consumed += 2; + break; + + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); + break; + + case 'a': + /* Choose all channels */ + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + set_mask |= 1<<i; + } + break; + + case 'l': + /* Read in custom low value */ + if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) + usage("low PWM invalid"); + break; + case 'h': + /* Read in custom high value */ + pwm_high = strtoul(optarg, &ep, 0); + if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) + usage("high PWM invalid"); + break; + default: + usage(NULL); + } + } + + if (set_mask == 0) + usage("no channels chosen"); + + /* make sure no other source is publishing control values now */ + struct actuator_controls_s actuators; + int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + + /* clear changed flag */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators); + + /* wait 50 ms */ + usleep(50000); + + /* now expect nothing changed on that topic */ + bool orb_updated; + orb_check(act_sub, &orb_updated); + + if (orb_updated) { + errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" + "\tmultirotor_att_control stop\n" + "\tfw_att_control stop\n"); + } + + printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" + "\n" + "Make sure\n" + "\t - that the ESCs are not powered\n" + "\t - that safety is off (two short blinks)\n" + "\t - that the controllers are stopped\n" + "\n" + "Do you want to start calibration now: y or n?\n"); + + /* wait for user input */ + while (1) { + + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c == 'y' || c == 'Y') { + + break; + + } else if (c == 0x03 || c == 0x63 || c == 'q') { + printf("ESC calibration exited\n"); + exit(0); + + } else if (c == 'n' || c == 'N') { + printf("ESC calibration aborted\n"); + exit(0); + + } else { + printf("Unknown input, ESC calibration aborted\n"); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + /* open for ioctl only */ + int fd = open(dev, 0); + + if (fd < 0) + err(1, "can't open %s", dev); + + /* get number of channels available on the device */ + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* tell IO/FMU that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + + warnx("Outputs armed"); + + + /* wait for user confirmation */ + printf("\nHigh PWM set: %d\n" + "\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" + "\n", pwm_high); + fflush(stdout); + + while (1) { + /* set max PWM */ + for (unsigned i = 0; i < max_channels; i++) { + + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high); + + if (ret != OK) + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_high); + } + } + + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c == 13) { + + break; + + } else if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("ESC calibration exited"); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + printf("Low PWM set: %d\n" + "\n" + "Hit ENTER when finished\n" + "\n", pwm_low); + + while (1) { + + /* set disarmed PWM */ + for (unsigned i = 0; i < max_channels; i++) { + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low); + + if (ret != OK) + err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_low); + } + } + + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c == 13) { + + break; + + } else if (c == 0x03 || c == 0x63 || c == 'q') { + printf("ESC calibration exited\n"); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + /* disarm */ + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + + warnx("Outputs disarmed"); + + printf("ESC calibration finished\n"); + + exit(0); +} diff --git a/src/systemcmds/esc_calib/module.mk b/src/systemcmds/esc_calib/module.mk new file mode 100644 index 000000000..990c56768 --- /dev/null +++ b/src/systemcmds/esc_calib/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 esc_calib tool. +# + +MODULE_COMMAND = esc_calib +SRCS = esc_calib.c + +MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/i2c/i2c.c b/src/systemcmds/i2c/i2c.c index 4da238039..34405c496 100644 --- a/src/systemcmds/i2c/i2c.c +++ b/src/systemcmds/i2c/i2c.c @@ -52,7 +52,7 @@ #include <nuttx/i2c.h> -#include <arch/board/board.h> +#include <board_config.h> #include "systemlib/systemlib.h" #include "systemlib/err.h" diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp index e642ed067..6da39d371 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.cpp @@ -47,10 +47,15 @@ #include <nuttx/compiler.h> #include <systemlib/err.h> -#include <drivers/drv_mixer.h> +#include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> -__EXPORT int mixer_main(int argc, char *argv[]); +/** + * Mixer utility for loading mixer files to devices + * + * @ingroup apps + */ +extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; static void load(const char *devname, const char *fname) noreturn_function; @@ -87,8 +92,6 @@ static void load(const char *devname, const char *fname) { int dev; - FILE *fp; - char line[120]; char buf[2048]; /* open the device */ @@ -99,49 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* read valid lines from the file into a buffer */ - buf[0] = '\0'; - for (;;) { - - /* get a line, bail on error/EOF */ - line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) - break; - - /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) - continue; - - /* compact whitespace in the buffer */ - char *t, *f; - for (f = buf; *f != '\0'; f++) { - /* scan for space characters */ - if (*f == ' ') { - /* look for additional spaces */ - t = f + 1; - while (*t == ' ') - t++; - if (*t == '\0') { - /* strip trailing whitespace */ - *f = '\0'; - } else if (t > (f + 1)) { - memmove(f + 1, t, strlen(t) + 1); - } - } - } - - /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index d5a3f9f77..cdbff75f0 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -36,7 +36,7 @@ # MODULE_COMMAND = mixer -SRCS = mixer.c +SRCS = mixer.cpp MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk new file mode 100644 index 000000000..a48588535 --- /dev/null +++ b/src/systemcmds/nshterm/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 nshterm utility +# + +MODULE_COMMAND = nshterm +SRCS = nshterm.c + +MODULE_STACKSIZE = 3000 diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c new file mode 100644 index 000000000..458bb2259 --- /dev/null +++ b/src/systemcmds/nshterm/nshterm.c @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Andrew Tridgell + * + * 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 nshterm.c + * start a nsh terminal on a given port. This can be useful for error + * handling in startup scripts to start a nsh shell on /dev/ttyACM0 + * for diagnostics + */ + +#include <nuttx/config.h> +#include <termios.h> +#include <stdbool.h> +#include <stdio.h> +#include <stdarg.h> +#include <unistd.h> +#include <stdlib.h> +#include <errno.h> +#include <apps/nsh.h> +#include <fcntl.h> +#include <systemlib/err.h> + +__EXPORT int nshterm_main(int argc, char *argv[]); + +int +nshterm_main(int argc, char *argv[]) +{ + if (argc < 2) { + printf("Usage: nshterm <device>\n"); + exit(1); + } + uint8_t retries = 0; + int fd = -1; + while (retries < 50) { + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ + fd = open(argv[1], O_RDWR); + if (fd != -1) { + break; + } + usleep(100000); + retries++; + } + if (fd == -1) { + perror(argv[1]); + exit(1); + } + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ + close(0); + close(1); + close(2); + dup2(fd, 0); + dup2(fd, 1); + dup2(fd, 2); + + nsh_consolemain(0, NULL); + + close(fd); + + return OK; +} diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index d1dd85d47..07581459b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include <drivers/drv_baro.h> #include <mavlink/mavlink_log.h> +#include <systemlib/rc_check.h> __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -108,7 +109,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- ACCEL ---- */ close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { @@ -118,6 +119,29 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + /* ---- GYRO ---- */ close(fd); @@ -139,101 +163,11 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + /* warn */ + if (!rc_ok) + warnx("rc calibration test failed"); /* require RC ok to keep system_ok */ system_ok &= rc_ok; @@ -249,7 +183,10 @@ system_eval: } else { fflush(stdout); - int buzzer = open("/dev/tone_alarm", O_WRONLY); + warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); + fflush(stderr); + + int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { @@ -263,25 +200,26 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 150; i++) + for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); if (i % 10 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 4); + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { - ioctl(buzzer, TONE_SET_ALARM, 2); + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ - ioctl(buzzer, TONE_SET_ALARM, 0); + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - /* switch on leds */ + /* switch off leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); + close(leds); if (fail_on_error) { /* exit with error message */ @@ -313,4 +251,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -}
\ No newline at end of file +} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index e150b5a74..09a934400 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -45,6 +45,7 @@ #include <stdbool.h> #include <unistd.h> #include <fcntl.h> +#include <poll.h> #include <sys/mount.h> #include <sys/ioctl.h> #include <sys/stat.h> @@ -71,16 +72,36 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n" - " -v Print information about the PWM device\n" - " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" - " <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n" - " <chanmask> Directly supply alt rate channel mask (debug use only)\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + " arm Arm output\n" + " disarm Disarm output\n" + "\n" + " rate ... Configure PWM rates\n" + " [-g <channel group>] Channel group that should update at the alternate rate\n" + " [-m <chanmask> ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -r <alt_rate> PWM rate (50 to 400 Hz)\n" + "\n" + " failsafe ... Configure failsafe PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Configure all outputs\n" + " -p <pwm value> PWM value\n" + "\n" + " test ... Directly set PWM values\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Configure all outputs\n" + " -p <pwm value> PWM value\n" + "\n" + " info Print information about the PWM device\n" + "\n" + " -v Print verbose information\n" + " -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -92,149 +113,389 @@ pwm_main(int argc, char *argv[]) unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; - bool print_info = false; + bool print_verbose = false; int ch; int ret; char *ep; + uint32_t set_mask = 0; unsigned group; - int32_t set_mask = -1; + unsigned long channels; + unsigned single_ch = 0; + unsigned pwm_value = 0; - if (argc < 2) + if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { + + case 'd': + if (NULL == strstr(optarg, "/dev/")) { + warnx("device %s not valid", optarg); + usage(NULL); + } + dev = optarg; + break; + + case 'v': + print_verbose = true; + break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'g': group = strtoul(optarg, &ep, 0); if ((*ep != '\0') || (group >= 32)) usage("bad channel_group value"); alt_channel_groups |= (1 << group); alt_channels_set = true; + warnx("alt channels set, group: %d", group); break; - case 'd': - dev = optarg; + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); break; - case 'u': - alt_rate = strtol(optarg, &ep, 0); + case 'a': + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + set_mask |= 1<<i; + } + break; + case 'p': + pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alt_rate value"); + usage("bad PWM value provided"); break; - - case 'm': - set_mask = strtol(optarg, &ep, 0); + case 'r': + alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("bad alternative rate provided"); break; - - case 'v': - print_info = true; + default: break; + } + } - default: - usage(NULL); + if (print_verbose && set_mask > 0) { + warnx("Chose channels: "); + printf(" "); + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + if (set_mask & 1<<i) + printf("%d ", i+1); } + printf("\n"); } - argc -= optind; - argv += optind; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + /* get the number of servo channels */ + unsigned servo_count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + if (!strcmp(argv[1], "arm")) { + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + + if (print_verbose) + warnx("Outputs armed"); + exit(0); - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + } else if (!strcmp(argv[1], "disarm")) { + /* disarm, but do not revoke the SET_ARM_OK flag */ + ret = ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + err(1, "PWM_SERVO_DISARM"); + + if (print_verbose) + warnx("Outputs disarmed"); + exit(0); + + } else if (!strcmp(argv[1], "rate")) { + + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); + } - for (unsigned group = 0; group < 32; group++) { - if ((1 << group) & alt_channel_groups) { - uint32_t group_mask; + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) - err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; - mask |= group_mask; + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } } + + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); } + exit(0); - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + } else if (!strcmp(argv[1], "min")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - /* iterate remaining arguments */ - unsigned channel = 0; - while (argc--) { - const char *arg = argv[0]; - argv++; - if (!strcmp(arg, "arm")) { - /* tell IO that its ok to disable its safety with the switch */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: min PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; + } + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); - /* tell IO that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); + errx(ret, "failed setting min values"); + } + exit(0); + + } else if (!strcmp(argv[1], "max")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: max PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; + } + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); if (ret != OK) - err(1, "PWM_SERVO_ARM"); - continue; + errx(ret, "failed setting max values"); + } + exit(0); + + } else if (!strcmp(argv[1], "disarmed")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + warnx("reading disarmed value of zero, disabling disarmed PWM"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("channel %d: disarmed PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; } - if (!strcmp(arg, "disarm")) { - /* disarm, but do not revoke the SET_ARM_OK flag */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - continue; + errx(ret, "failed setting disarmed values"); } - unsigned pwm_value = strtol(arg, &ep, 0); - if (*ep == '\0') { - ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); + exit(0); + + } else if (!strcmp(argv[1], "failsafe")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; + } + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values); if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", channel); - channel++; - continue; + errx(ret, "failed setting failsafe values"); } - usage("unrecognised option"); - } + exit(0); - /* print verbose info */ - if (print_info) { - /* get the number of servo channels */ - unsigned count; - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + } else if (!strcmp(argv[1], "test")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + /* perform PWM output */ + + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + } + + /* abort on user request */ + char c; + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("User abort\n"); + exit(0); + } + } + } + exit(0); + + + } else if (!strcmp(argv[1], "info")) { + + printf("device: %s\n", dev); + + uint32_t info_default_rate; + uint32_t info_alt_rate; + uint32_t info_alt_rate_mask; + + ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask); + if (ret != OK) { + err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); + } + + struct pwm_output_values failsafe_pwm; + struct pwm_output_values disarmed_pwm; + struct pwm_output_values min_pwm; + struct pwm_output_values max_pwm; + + ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_FAILSAFE_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MAX_PWM"); + } /* print current servo values */ - for (unsigned i = 0; i < count; i++) { + for (unsigned i = 0; i < servo_count; i++) { servo_position_t spos; ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); if (ret == OK) { - printf("channel %u: %uus\n", i, spos); + printf("channel %u: %u us", i+1, spos); + + if (info_alt_rate_mask & (1<<i)) + printf(" (alternative rate: %d Hz", info_alt_rate); + else + printf(" (default rate: %d Hz", info_default_rate); + + + printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)", + failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]); + printf("\n"); } else { printf("%u: ERROR\n", i); } } - /* print rate groups */ - for (unsigned i = 0; i < count; i++) { + for (unsigned i = 0; i < servo_count; i++) { uint32_t group_mask; ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); @@ -244,11 +505,14 @@ pwm_main(int argc, char *argv[]) printf("channel group %u: channels", i); for (unsigned j = 0; j < 32; j++) if (group_mask & (1 << j)) - printf(" %u", j); + printf(" %u", j+1); printf("\n"); } } - fflush(stdout); + exit(0); + } - exit(0); -}
\ No newline at end of file + usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); + return 0; +} + diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 47d3cd948..91a2c2eb8 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -40,14 +40,31 @@ #include <nuttx/config.h> #include <unistd.h> #include <stdio.h> +#include <getopt.h> #include <systemlib/systemlib.h> +#include <systemlib/err.h> __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { - up_systemreset(); + int ch; + bool to_bootloader = false; + + while ((ch = getopt(argc, argv, "b")) != -1) { + switch (ch) { + case 'b': + to_bootloader = true; + break; + default: + errx(1, "usage: reboot [-b]\n" + " -b reboot into the bootloader"); + + } + } + + systemreset(to_bootloader); } diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 754d3a0da..6729ce4ae 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -23,6 +23,7 @@ SRCS = test_adc.c \ test_uart_console.c \ test_uart_loopback.c \ test_uart_send.c \ + test_mixer.cpp \ tests_file.c \ tests_main.c \ tests_param.c diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f6e540401..5690997a9 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) { - printf("failed opening /dev/tone_alarm\n"); + printf("failed opening " TONEALARM_DEVICE_PATH "\n"); goto out; } @@ -137,7 +137,7 @@ int test_tone(int argc, char *argv[]) tone = atoi(argv[1]); if (tone == 0) { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); @@ -148,7 +148,7 @@ int test_tone(int argc, char *argv[]) } } else { - result = ioctl(fd, TONE_SET_ALARM, 0); + result = ioctl(fd, TONE_SET_ALARM, TONE_STOP_TUNE); if (result < 0) { printf("failed clearing alarms\n"); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp new file mode 100644 index 000000000..4da86042d --- /dev/null +++ b/src/systemcmds/tests/test_mixer.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * 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 test_mixer.hpp + * + * Mixer load test + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <string.h> +#include <time.h> + +#include <systemlib/err.h> +#include <systemlib/mixer/mixer.h> + +#include "tests.h" + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +int test_mixer(int argc, char *argv[]) +{ + warnx("testing mixer"); + + char *filename = "/etc/mixers/IO_pass.mix"; + + if (argc > 1) + filename = argv[1]; + + warnx("loading: %s", filename); + + char buf[2048]; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + /* load the mixer in chunks, like + * in the case of a remote load, + * e.g. on PX4IO. + */ + + unsigned nused = 0; + + const unsigned chunk_size = 64; + + MixerGroup mixer_group(mixer_callback, 0); + + /* load at once test */ + unsigned xx = loaded; + mixer_group.load_from_buf(&buf[0], xx); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; + + unsigned empty_load = 2; + char empty_buf[2]; + empty_buf[0] = ' '; + empty_buf[1] = '\0'; + mixer_group.reset(); + mixer_group.load_from_buf(&empty_buf[0], empty_load); + warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + if (empty_load != 0) + return 1; + + /* FIRST mark the mixer as invalid */ + bool mixer_ok = false; + /* THEN actually delete it */ + mixer_group.reset(); + char mixer_text[256]; /* large enough for one mixer */ + unsigned mixer_text_length = 0; + + unsigned transmitted = 0; + + warnx("transmitted: %d, loaded: %d", transmitted, loaded); + + while (transmitted < loaded) { + + unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + bool mixer_ok = false; + return 1; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + mixer_ok = true; + } else { + /* not yet reached the end of the mixer, set as not ok */ + mixer_ok = false; + } + + warnx("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + transmitted += text_length; + } + + warnx("chunked load: loaded %u mixers", mixer_group.count()); + + if (mixer_group.count() != 8) + return 1; + + /* load multirotor at once test */ + mixer_group.reset(); + + if (argc > 2) + filename = argv[2]; + else + filename = "/etc/mixers/FMU_quad_w.mix"; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + unsigned mc_loaded = loaded; + mixer_group.load_from_buf(&buf[0], mc_loaded); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; + + control = 0.0f; + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c02ea6808..c483108cf 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -76,6 +76,8 @@ * Public Function Prototypes ****************************************************************************/ +__BEGIN_DECLS + extern int test_sensors(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]); @@ -98,5 +100,8 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_mixer(int argc, char *argv[]); + +__END_DECLS #endif /* __APPS_PX4_TESTS_H */ diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 6f75b9812..7f3a654bf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -38,7 +38,9 @@ */ #include <sys/stat.h> +#include <dirent.h> #include <stdio.h> +#include <stddef.h> #include <unistd.h> #include <fcntl.h> #include <systemlib/err.h> @@ -52,6 +54,224 @@ int test_file(int argc, char *argv[]) { + const unsigned iterations = 100; + const unsigned alignments = 65; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + for (unsigned a = 0; a < alignments; a++) { + + printf("\n"); + warnx("----- alignment test: %u bytes -----", a); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing unaligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); + + } + + fsync(fd); + //perf_end(wperf); + + } + end = hrt_absolute_time(); + + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + /* + * ALIGNED WRITES AND UNALIGNED READS + */ + + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + for (int j = 0; j < chunk_sizes[c]; j++) { + + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); + } + + return 0; +} +#if 0 +int +test_file(int argc, char *argv[]) +{ + const iterations = 1024; + /* check if microSD card is mounted */ struct stat buffer; if (stat("/fs/microsd/", &buffer)) { @@ -67,7 +287,7 @@ test_file(int argc, char *argv[]) memset(buf, 0, sizeof(buf)); start = hrt_absolute_time(); - for (unsigned i = 0; i < 1024; i++) { + for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); write(fd, buf, sizeof(buf)); perf_end(wperf); @@ -76,9 +296,40 @@ test_file(int argc, char *argv[]) close(fd); - warnx("512KiB in %llu microseconds", end - start); + unlink("/fs/microsd/testfile"); + + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); perf_print_counter(wperf); perf_free(wperf); + warnx("running unlink test"); + + /* ensure that common commands do not run against file count limits */ + for (unsigned i = 0; i < 64; i++) { + + warnx("unlink iteration #%u", i); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file before unlink()"); + int ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file before unlink()"); + close(fd); + + ret = unlink("/fs/microsd/testfile"); + if (ret != OK) + errx(1, "failed unlinking test file"); + + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + if (fd < 0) + errx(1, "failed opening test file after unlink()"); + ret = write(fd, buf, sizeof(buf)); + if (ret < 0) + errx(1, "failed writing test file after unlink()"); + close(fd); + } + return 0; } +#endif diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9f8c5c9ea..9eb9c632e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,6 +110,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05e..1ca3fc928 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) float interval_time_ms_inv = 0.f; - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } |