mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Deleted OpenPilot.posix branch. Integrated portability modifications into main tree
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1086 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
parent
f56866c45b
commit
fa57e42efa
@ -1,728 +0,0 @@
|
||||
#####
|
||||
# Project: OpenPilot
|
||||
#
|
||||
#
|
||||
# Makefile for OpenPilot project build PiOS and the AP.
|
||||
#
|
||||
# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
|
||||
#
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#####
|
||||
|
||||
|
||||
# Set developer code and compile options
|
||||
# Set to YES to compile for debugging
|
||||
DEBUG ?= YES
|
||||
|
||||
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
|
||||
ENABLE_DEBUG_PINS ?= NO
|
||||
|
||||
# Set to Yes to enable the AUX UART which is mapped on the S1 (Tx) and S2 (Rx) servo outputs
|
||||
ENABLE_AUX_UART ?= NO
|
||||
|
||||
#
|
||||
USE_BOOTLOADER ?= NO
|
||||
|
||||
|
||||
# Set to YES when using Code Sourcery toolchain
|
||||
CODE_SOURCERY ?= YES
|
||||
|
||||
# Toolchain prefix (i.e arm-elf- -> arm-elf-gcc.exe)
|
||||
TCHAIN_PREFIX ?= arm-none-eabi-
|
||||
|
||||
# Remove command is different for Code Sourcery on Windows
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
REMOVE_CMD = cs-rm
|
||||
else
|
||||
REMOVE_CMD = rm
|
||||
endif
|
||||
|
||||
FLASH_TOOL = OPENOCD
|
||||
|
||||
# YES enables -mthumb option to flags for source-files listed
|
||||
# in SRC and CPPSRC
|
||||
USE_THUMB_MODE = YES
|
||||
|
||||
# List of modules to include
|
||||
MODULES = Telemetry GPS ManualControl Actuator Altitude AHRSComms Stabilization
|
||||
#MODULES = Telemetry Example
|
||||
#MODULES = Telemetry MK/MKSerial
|
||||
|
||||
#MODULES += Osd/OsdEtStd
|
||||
|
||||
|
||||
# MCU name, submodel and board
|
||||
# - MCU used for compiler-option (-mcpu)
|
||||
# - MODEL used for linker-script name (-T) and passed as define
|
||||
# - BOARD just passed as define (optional)
|
||||
MCU = cortex-m3
|
||||
CHIP = STM32F103RET
|
||||
BOARD = STM3210E_OP
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
MODEL = HD_BL
|
||||
|
||||
else
|
||||
MODEL = HD_NB
|
||||
endif
|
||||
|
||||
# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
|
||||
OUTDIR = Build
|
||||
|
||||
# Target file name (without extension).
|
||||
TARGET = OpenPilot
|
||||
|
||||
# Paths
|
||||
OPSYSTEM = ./System
|
||||
OPSYSTEMINC = $(OPSYSTEM)/inc
|
||||
OPUAVTALK = ./UAVTalk
|
||||
OPUAVTALKINC = $(OPUAVTALK)/inc
|
||||
OPUAVOBJ = ./UAVObjects
|
||||
OPUAVOBJINC = $(OPUAVOBJ)/inc
|
||||
OPTESTS = ./Tests
|
||||
OPMODULEDIR = ./Modules
|
||||
PIOS = ../PiOS
|
||||
PIOSINC = $(PIOS)/inc
|
||||
PIOSSTM32F10X = $(PIOS)/STM32F10x
|
||||
PIOSCOMMON = $(PIOS)/Common
|
||||
APPLIBDIR = $(PIOSSTM32F10X)/Libraries
|
||||
STMLIBDIR = $(APPLIBDIR)
|
||||
STMSPDDIR = $(STMLIBDIR)/STM32F10x_StdPeriph_Driver
|
||||
STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver
|
||||
STMSPDSRCDIR = $(STMSPDDIR)/src
|
||||
STMSPDINCDIR = $(STMSPDDIR)/inc
|
||||
STMUSBSRCDIR = $(STMUSBDIR)/src
|
||||
STMUSBINCDIR = $(STMUSBDIR)/inc
|
||||
CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3
|
||||
DOSFSDIR = $(APPLIBDIR)/dosfs
|
||||
MSDDIR = $(APPLIBDIR)/msd
|
||||
RTOSDIR = $(APPLIBDIR)/FreeRTOS
|
||||
RTOSSRCDIR = $(RTOSDIR)/Source
|
||||
RTOSINCDIR = $(RTOSSRCDIR)/include
|
||||
DOXYGENDIR = ../Doc/Doxygen
|
||||
|
||||
# List C source files here. (C dependencies are automatically generated.)
|
||||
# use file-extension c for "c-only"-files
|
||||
|
||||
MODNAMES = $(notdir ${MODULES})
|
||||
|
||||
ifndef TESTAPP
|
||||
## MODULES
|
||||
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
|
||||
SRC += ${OUTDIR}/InitMods.c
|
||||
## OPENPILOT CORE:
|
||||
SRC += ${OPMODULEDIR}/System/systemmod.c
|
||||
SRC += $(OPSYSTEM)/openpilot.c
|
||||
SRC += $(OPSYSTEM)/pios_board.c
|
||||
SRC += $(OPSYSTEM)/alarms.c
|
||||
SRC += $(OPUAVTALK)/uavtalk.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectmanager.c
|
||||
SRC += $(OPUAVOBJ)/uavobjectsinit.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
else
|
||||
## TESTCODE
|
||||
SRC += $(OPTESTS)/test_common.c
|
||||
SRC += $(OPTESTS)/$(TESTAPP).c
|
||||
endif
|
||||
|
||||
|
||||
|
||||
## UAVOBJECTS
|
||||
ifndef TESTAPP
|
||||
SRC += $(OPUAVOBJ)/exampleobject1.c
|
||||
SRC += $(OPUAVOBJ)/exampleobject2.c
|
||||
SRC += $(OPUAVOBJ)/examplesettings.c
|
||||
SRC += $(OPUAVOBJ)/objectpersistence.c
|
||||
SRC += $(OPUAVOBJ)/positionactual.c
|
||||
SRC += $(OPUAVOBJ)/gcstelemetrystats.c
|
||||
SRC += $(OPUAVOBJ)/flighttelemetrystats.c
|
||||
SRC += $(OPUAVOBJ)/systemstats.c
|
||||
SRC += $(OPUAVOBJ)/systemalarms.c
|
||||
SRC += $(OPUAVOBJ)/systemsettings.c
|
||||
SRC += $(OPUAVOBJ)/telemetrysettings.c
|
||||
SRC += $(OPUAVOBJ)/actuatorcommand.c
|
||||
SRC += $(OPUAVOBJ)/actuatordesired.c
|
||||
SRC += $(OPUAVOBJ)/actuatorsettings.c
|
||||
SRC += $(OPUAVOBJ)/manualcontrolcommand.c
|
||||
SRC += $(OPUAVOBJ)/manualcontrolsettings.c
|
||||
SRC += $(OPUAVOBJ)/attitudedesired.c
|
||||
SRC += $(OPUAVOBJ)/stabilizationsettings.c
|
||||
SRC += $(OPUAVOBJ)/ahrsstatus.c
|
||||
SRC += $(OPUAVOBJ)/altitudeactual.c
|
||||
SRC += $(OPUAVOBJ)/attitudeactual.c
|
||||
SRC += $(OPUAVOBJ)/attitudesettings.c
|
||||
SRC += $(OPUAVOBJ)/flightbatterystate.c
|
||||
SRC += $(OPUAVOBJ)/headingactual.c
|
||||
endif
|
||||
|
||||
## PIOS Hardware (STM32F10x)
|
||||
SRC += $(PIOSSTM32F10X)/pios_sys.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_led.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_delay.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usart.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_irq.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_adc.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_servo.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_i2c.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spi.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_ppm.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_pwm.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_spektrum.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_usb_hid.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_debug.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_gpio.c
|
||||
SRC += $(PIOSSTM32F10X)/pios_exti.c
|
||||
|
||||
## PIOS Hardware (Common)
|
||||
SRC += $(PIOSCOMMON)/pios_sdcard.c
|
||||
SRC += $(PIOSCOMMON)/pios_com.c
|
||||
SRC += $(PIOSCOMMON)/pios_bmp085.c
|
||||
SRC += $(PIOSCOMMON)/pios_opahrs.c
|
||||
SRC += $(PIOSCOMMON)/pios_opahrs_proto.c
|
||||
SRC += $(PIOSCOMMON)/printf-stdarg.c
|
||||
|
||||
## CMSIS for STM32
|
||||
SRC += $(CMSISDIR)/core_cm3.c
|
||||
SRC += $(CMSISDIR)/system_stm32f10x.c
|
||||
|
||||
## Used parts of the STM-Library
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_adc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_bkp.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_crc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dac.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_dma.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_exti.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_flash.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_gpio.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_i2c.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_pwr.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rcc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_rtc.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_spi.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_tim.c
|
||||
SRC += $(STMSPDSRCDIR)/stm32f10x_usart.c
|
||||
SRC += $(STMSPDSRCDIR)/misc.c
|
||||
|
||||
## STM32 USB Library
|
||||
SRC += $(STMUSBSRCDIR)/usb_core.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_init.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_int.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_mem.c
|
||||
SRC += $(STMUSBSRCDIR)/usb_regs.c
|
||||
|
||||
## RTOS
|
||||
SRC += $(RTOSSRCDIR)/list.c
|
||||
SRC += $(RTOSSRCDIR)/queue.c
|
||||
SRC += $(RTOSSRCDIR)/tasks.c
|
||||
|
||||
## RTOS Portable
|
||||
SRC += $(RTOSSRCDIR)/portable/GCC/ARM_CM3/port.c
|
||||
SRC += $(RTOSSRCDIR)/portable/MemMang/heap_2.c
|
||||
|
||||
## Dosfs file system
|
||||
SRC += $(DOSFSDIR)/dosfs.c
|
||||
SRC += $(DOSFSDIR)/dfs_sdcard.c
|
||||
|
||||
## Mass Storage Device
|
||||
SRC += $(MSDDIR)/msd.c
|
||||
SRC += $(MSDDIR)/msd_bot.c
|
||||
SRC += $(MSDDIR)/msd_desc.c
|
||||
SRC += $(MSDDIR)/msd_memory.c
|
||||
SRC += $(MSDDIR)/msd_scsi.c
|
||||
SRC += $(MSDDIR)/msd_scsi_data.c
|
||||
|
||||
# List C source files here which must be compiled in ARM-Mode (no -mthumb).
|
||||
# use file-extension c for "c-only"-files
|
||||
## just for testing, timer.c could be compiled in thumb-mode too
|
||||
SRCARM =
|
||||
|
||||
# List C++ source files here.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
CPPSRC =
|
||||
|
||||
# List C++ source files here which must be compiled in ARM-Mode.
|
||||
# use file-extension .cpp for C++-files (not .C)
|
||||
#CPPSRCARM = $(TARGET).cpp
|
||||
CPPSRCARM =
|
||||
|
||||
# List Assembler source files here.
|
||||
# Make them always end in a capital .S. Files ending in a lowercase .s
|
||||
# will not be considered source files but generated files (assembler
|
||||
# output from the compiler), and will be deleted upon "make clean"!
|
||||
# Even though the DOS/Win* filesystem matches both .s and .S the same,
|
||||
# it will preserve the spelling of the filenames, and gcc itself does
|
||||
# care about how the name is spelled on its command-line.
|
||||
ASRC = $(PIOSSTM32F10X)/startup_stm32f10x_$(MODEL).S
|
||||
|
||||
# List Assembler source files here which must be assembled in ARM-Mode..
|
||||
ASRCARM =
|
||||
|
||||
# List any extra directories to look for include files here.
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRAINCDIRS = $(OPSYSTEM)
|
||||
EXTRAINCDIRS += $(OPSYSTEMINC)
|
||||
EXTRAINCDIRS += $(OPUAVTALK)
|
||||
EXTRAINCDIRS += $(OPUAVTALKINC)
|
||||
EXTRAINCDIRS += $(OPUAVOBJ)
|
||||
EXTRAINCDIRS += $(OPUAVOBJINC)
|
||||
EXTRAINCDIRS += $(PIOS)
|
||||
EXTRAINCDIRS += $(PIOSINC)
|
||||
EXTRAINCDIRS += $(PIOSSTM32F10X)
|
||||
EXTRAINCDIRS += $(PIOSCOMMON)
|
||||
EXTRAINCDIRS += $(STMSPDINCDIR)
|
||||
EXTRAINCDIRS += $(STMUSBINCDIR)
|
||||
EXTRAINCDIRS += $(CMSISDIR)
|
||||
EXTRAINCDIRS += $(DOSFSDIR)
|
||||
EXTRAINCDIRS += $(MSDDIR)
|
||||
EXTRAINCDIRS += $(RTOSINCDIR)
|
||||
EXTRAINCDIRS += $(APPLIBDIR)
|
||||
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3
|
||||
|
||||
EXTRAINCDIRS += ${foreach MOD, ${MODULES}, Modules/${MOD}/inc} ${OPMODULEDIR}/System/inc
|
||||
|
||||
|
||||
# List any extra directories to look for library files here.
|
||||
# Also add directories where the linker should search for
|
||||
# includes from linker-script to the list
|
||||
# Each directory must be seperated by a space.
|
||||
EXTRA_LIBDIRS =
|
||||
|
||||
# Extra Libraries
|
||||
# Each library-name must be seperated by a space.
|
||||
# i.e. to link with libxyz.a, libabc.a and libefsl.a:
|
||||
# EXTRA_LIBS = xyz abc efsl
|
||||
# for newlib-lpc (file: libnewlibc-lpc.a):
|
||||
# EXTRA_LIBS = newlib-lpc
|
||||
EXTRA_LIBS =
|
||||
|
||||
# Path to Linker-Scripts
|
||||
LINKERSCRIPTPATH = $(PIOSSTM32F10X)
|
||||
|
||||
# Optimization level, can be [0, 1, 2, 3, s].
|
||||
# 0 = turn off optimization. s = optimize for size.
|
||||
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
OPT = 0
|
||||
else
|
||||
OPT = s
|
||||
endif
|
||||
|
||||
# Output format. (can be ihex or binary or both)
|
||||
# binary to create a load-image in raw-binary format i.e. for SAM-BA,
|
||||
# ihex to create a load-image in Intel hex format
|
||||
#LOADFORMAT = ihex
|
||||
#LOADFORMAT = binary
|
||||
LOADFORMAT = both
|
||||
|
||||
# Debugging format.
|
||||
DEBUGF = dwarf-2
|
||||
|
||||
# Place project-specific -D (define) and/or
|
||||
# -U options for C here.
|
||||
CDEFS = -DSTM32F10X_$(MODEL)
|
||||
CDEFS += -DUSE_STDPERIPH_DRIVER
|
||||
CDEFS += -DUSE_$(BOARD)
|
||||
ifeq ($(ENABLE_DEBUG_PINS), YES)
|
||||
CDEFS += -DPIOS_ENABLE_DEBUG_PINS
|
||||
endif
|
||||
ifeq ($(ENABLE_AUX_UART), YES)
|
||||
CDEFS += -DPIOS_ENABLE_AUX_UART
|
||||
endif
|
||||
ifeq ($(USE_BOOTLOADER), YES)
|
||||
CDEFS += -DUSE_BOOTLOADER
|
||||
endif
|
||||
|
||||
|
||||
# Place project-specific -D and/or -U options for
|
||||
# Assembler with preprocessor here.
|
||||
#ADEFS = -DUSE_IRQ_ASM_WRAPPER
|
||||
ADEFS = -D__ASSEMBLY__
|
||||
|
||||
# Compiler flag to set the C Standard level.
|
||||
# c89 - "ANSI" C
|
||||
# gnu89 - c89 plus GCC extensions
|
||||
# c99 - ISO C99 standard (not yet fully implemented)
|
||||
# gnu99 - c99 plus GCC extensions
|
||||
CSTANDARD = -std=gnu99
|
||||
|
||||
#-----
|
||||
|
||||
# Compiler flags.
|
||||
|
||||
# -g*: generate debugging information
|
||||
# -O*: optimization level
|
||||
# -f...: tuning, see GCC manual and avr-libc documentation
|
||||
# -Wall...: warning level
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -adhlns...: create assembler listing
|
||||
#
|
||||
# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
|
||||
|
||||
ifeq ($(DEBUG),YES)
|
||||
CFLAGS = -g$(DEBUGF) -DDEBUG
|
||||
endif
|
||||
|
||||
CFLAGS += -O$(OPT)
|
||||
CFLAGS += -mcpu=$(MCU) -mthumb
|
||||
CFLAGS += $(CDEFS)
|
||||
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
|
||||
|
||||
CFLAGS += -mapcs-frame
|
||||
CFLAGS += -fomit-frame-pointer
|
||||
ifeq ($(CODE_SOURCERY), YES)
|
||||
CFLAGS += -fpromote-loop-indices
|
||||
endif
|
||||
|
||||
CFLAGS += -Wall
|
||||
CFLAGS += -Werror
|
||||
CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
# Compiler flags to generate dependency files:
|
||||
CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
|
||||
|
||||
# flags only for C
|
||||
#CONLYFLAGS += -Wnested-externs
|
||||
CONLYFLAGS += $(CSTANDARD)
|
||||
|
||||
# Assembler flags.
|
||||
# -Wa,...: tell GCC to pass this to the assembler.
|
||||
# -ahlns: create listing
|
||||
ASFLAGS = -mcpu=$(MCU) -mthumb -I. -x assembler-with-cpp
|
||||
ASFLAGS += $(ADEFS)
|
||||
ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
|
||||
ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
|
||||
|
||||
MATH_LIB = -lm
|
||||
|
||||
# Linker flags.
|
||||
# -Wl,...: tell GCC to pass this to linker.
|
||||
# -Map: create map file
|
||||
# --cref: add cross reference to map file
|
||||
LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
|
||||
LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
|
||||
LDFLAGS += -lc
|
||||
LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
|
||||
LDFLAGS += $(MATH_LIB)
|
||||
LDFLAGS += -lc -lgcc
|
||||
|
||||
# Set linker-script name depending on selected submodel name
|
||||
LDFLAGS +=-T$(LINKERSCRIPTPATH)/link_stm32f10x_$(MODEL).ld
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Options for OpenOCD flash-programming
|
||||
# see openocd.pdf/openocd.texi for further information
|
||||
#
|
||||
OOCD_LOADFILE+=$(OUTDIR)/$(TARGET).elf
|
||||
# if OpenOCD is in the $PATH just set OOCD_EXE=openocd
|
||||
OOCD_EXE=openocd
|
||||
# debug level
|
||||
OOCD_CL=-d0
|
||||
# interface and board/target settings (using the OOCD target-library here)
|
||||
OOCD_CL+=-f ../Project/OpenOCD/floss-jtag.openpilot.cfg -f ../Project/OpenOCD/stm32.cfg
|
||||
# initialize
|
||||
OOCD_CL+=-c init
|
||||
# show the targets
|
||||
OOCD_CL+=-c targets
|
||||
# commands to prepare flash-write
|
||||
OOCD_CL+= -c "reset halt"
|
||||
# flash erase
|
||||
OOCD_CL+=-c "stm32x mass_erase 0"
|
||||
# flash-write
|
||||
OOCD_CL+=-c "flash write_image $(OOCD_LOADFILE)"
|
||||
# Verify
|
||||
OOCD_CL+=-c "verify_image $(OOCD_LOADFILE)"
|
||||
# reset target
|
||||
OOCD_CL+=-c "reset run"
|
||||
# terminate OOCD after programming
|
||||
OOCD_CL+=-c shutdown
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
|
||||
# Define programs and commands.
|
||||
CC = $(TCHAIN_PREFIX)gcc
|
||||
CPP = $(TCHAIN_PREFIX)g++
|
||||
AR = $(TCHAIN_PREFIX)ar
|
||||
OBJCOPY = $(TCHAIN_PREFIX)objcopy
|
||||
OBJDUMP = $(TCHAIN_PREFIX)objdump
|
||||
SIZE = $(TCHAIN_PREFIX)size
|
||||
NM = $(TCHAIN_PREFIX)nm
|
||||
REMOVE = $(REMOVE_CMD) -f
|
||||
###SHELL = sh
|
||||
###COPY = cp
|
||||
|
||||
|
||||
|
||||
# Define Messages
|
||||
# English
|
||||
MSG_ERRORS_NONE = Errors: none
|
||||
MSG_BEGIN = ${quote}-------- begin (mode: $(RUN_MODE)) --------${quote}
|
||||
MSG_END = ${quote}-------- end --------${quote}
|
||||
MSG_MODINIT = ${quote}**** Generating ModInit.c${quote}
|
||||
MSG_SIZE_BEFORE = ${quote}Size before:${quote}
|
||||
MSG_SIZE_AFTER = ${quote}Size after build:${quote}
|
||||
MSG_LOAD_FILE = ${quote}Creating load file:${quote}
|
||||
MSG_EXTENDED_LISTING = ${quote}Creating Extended Listing/Disassembly:${quote}
|
||||
MSG_SYMBOL_TABLE = ${quote}Creating Symbol Table:${quote}
|
||||
MSG_LINKING = ${quote}**** Linking :${quote}
|
||||
MSG_COMPILING = ${quote}**** Compiling C :${quote}
|
||||
MSG_COMPILING_ARM = ${quote}**** Compiling C (ARM-only):${quote}
|
||||
MSG_COMPILINGCPP = ${quote}Compiling C++ :${quote}
|
||||
MSG_COMPILINGCPP_ARM = ${quote}Compiling C++ (ARM-only):${quote}
|
||||
MSG_ASSEMBLING = ${quote}**** Assembling:${quote}
|
||||
MSG_ASSEMBLING_ARM = ${quote}****Assembling (ARM-only):${quote}
|
||||
MSG_CLEANING = ${quote}Cleaning project:${quote}
|
||||
MSG_FORMATERROR = ${quote}Can not handle output-format${quote}
|
||||
MSG_ASMFROMC = ${quote}Creating asm-File from C-Source:${quote}
|
||||
MSG_ASMFROMC_ARM = ${quote}Creating asm-File from C-Source (ARM-only):${quote}
|
||||
|
||||
# List of all source files.
|
||||
ALLSRC = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
|
||||
# List of all source files without directory and file-extension.
|
||||
ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
|
||||
|
||||
# Define all object files.
|
||||
ALLOBJ = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
|
||||
|
||||
# Define all listing files (used for make clean).
|
||||
LSTFILES = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
|
||||
# Define all depedency-files (used for make clean).
|
||||
DEPFILES = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
|
||||
|
||||
elf: $(OUTDIR)/$(TARGET).elf
|
||||
lss: $(OUTDIR)/$(TARGET).lss
|
||||
sym: $(OUTDIR)/$(TARGET).sym
|
||||
hex: $(OUTDIR)/$(TARGET).hex
|
||||
bin: $(OUTDIR)/$(TARGET).bin
|
||||
|
||||
# Default target.
|
||||
#all: begin gccversion sizebefore build sizeafter finished end
|
||||
all: begin gccversion build sizeafter finished end
|
||||
|
||||
ifeq ($(LOADFORMAT),ihex)
|
||||
build: elf hex lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),binary)
|
||||
build: elf bin lss sym
|
||||
else
|
||||
ifeq ($(LOADFORMAT),both)
|
||||
build: elf hex bin lss sym
|
||||
else
|
||||
$(error "$(MSG_FORMATERROR) $(FORMAT)")
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
# Test if quotes are needed for the echo-command
|
||||
result = ${shell echo "test"}
|
||||
ifeq (${result}, test)
|
||||
quote = '
|
||||
else
|
||||
quote =
|
||||
endif
|
||||
|
||||
${OUTDIR}/InitMods.c: Makefile
|
||||
@echo ${MSG_MODINIT}
|
||||
@echo ${quote}// Autogenerated file${quote} > ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, extern unsigned int ${MOD}Initialize(void);}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}void InitModules() {${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}${foreach MOD, ${MODNAMES}, ${MOD}Initialize();}${quote} >> ${OUTDIR}/InitMods.c
|
||||
@echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c
|
||||
|
||||
# Eye candy.
|
||||
begin:
|
||||
## @echo
|
||||
@echo $(MSG_BEGIN)
|
||||
|
||||
finished:
|
||||
## @echo $(MSG_ERRORS_NONE)
|
||||
|
||||
end:
|
||||
@echo $(MSG_END)
|
||||
## @echo
|
||||
|
||||
# Display sizes of sections.
|
||||
ELFSIZE = $(SIZE) -A $(OUTDIR)/$(TARGET).elf
|
||||
##ELFSIZE = $(SIZE) --format=Berkeley --common $(OUTDIR)/$(TARGET).elf
|
||||
sizebefore:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
|
||||
|
||||
sizeafter:
|
||||
# @if [ -f $(OUTDIR)/$(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
|
||||
@echo $(MSG_SIZE_AFTER)
|
||||
$(ELFSIZE)
|
||||
|
||||
# Display compiler version information.
|
||||
gccversion :
|
||||
@$(CC) --version
|
||||
# @echo $(ALLOBJ)
|
||||
|
||||
# Program the device.
|
||||
ifeq ($(FLASH_TOOL),OPENOCD)
|
||||
# Program the device with Dominic Rath's OPENOCD in "batch-mode", needs cfg and "reset-script".
|
||||
program: $(OUTDIR)/$(TARGET).elf
|
||||
@echo ${quote}Programming with OPENOCD${quote}
|
||||
$(OOCD_EXE) $(OOCD_CL)
|
||||
endif
|
||||
|
||||
# Create final output file (.hex) from ELF output file.
|
||||
%.hex: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O ihex $< $@
|
||||
|
||||
# Create final output file (.bin) from ELF output file.
|
||||
%.bin: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_LOAD_FILE) $@
|
||||
$(OBJCOPY) -O binary $< $@
|
||||
|
||||
# Create extended listing file/disassambly from ELF output file.
|
||||
# using objdump testing: option -C
|
||||
%.lss: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_EXTENDED_LISTING) $@
|
||||
$(OBJDUMP) -h -S -C -r $< > $@
|
||||
# $(OBJDUMP) -x -S $< > $@
|
||||
|
||||
# Create a symbol table from ELF output file.
|
||||
%.sym: %.elf
|
||||
## @echo
|
||||
@echo $(MSG_SYMBOL_TABLE) $@
|
||||
$(NM) -n $< > $@
|
||||
|
||||
# Link: create ELF output file from object files.
|
||||
.SECONDARY : $(TARGET).elf
|
||||
.PRECIOUS : $(ALLOBJ)
|
||||
%.elf: $(ALLOBJ)
|
||||
@echo $(MSG_LINKING) $@
|
||||
# use $(CC) for C-only projects or $(CPP) for C++-projects:
|
||||
$(CC) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
# $(CPP) $(THUMB) $(CFLAGS) $(ALLOBJ) --output $@ $(LDFLAGS)
|
||||
|
||||
|
||||
# Assemble: create object files from assembler source files.
|
||||
define ASSEMBLE_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
|
||||
|
||||
# Assemble: create object files from assembler source files. ARM-only
|
||||
define ASSEMBLE_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_ASSEMBLING_ARM) $$< to $$@
|
||||
$(CC) -c $$(ASFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C source files.
|
||||
define COMPILE_C_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C source files. ARM-only
|
||||
define COMPILE_C_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILING_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CONLYFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create object files from C++ source files.
|
||||
define COMPILE_CPP_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP) $$< to $$@
|
||||
$(CC) -c $(THUMB) $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
|
||||
|
||||
# Compile: create object files from C++ source files. ARM-only
|
||||
define COMPILE_CPP_ARM_TEMPLATE
|
||||
$(OUTDIR)/$(notdir $(basename $(1))).o : $(1)
|
||||
## @echo
|
||||
@echo $(MSG_COMPILINGCPP_ARM) $$< to $$@
|
||||
$(CC) -c $$(CFLAGS) $$(CPPFLAGS) $$< -o $$@
|
||||
endef
|
||||
$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
|
||||
|
||||
|
||||
# Compile: create assembler files from C source files. ARM/Thumb
|
||||
$(SRC:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC) $< to $@
|
||||
$(CC) $(THUMB) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Compile: create assembler files from C source files. ARM only
|
||||
$(SRCARM:.c=.s) : %.s : %.c
|
||||
@echo $(MSG_ASMFROMC_ARM) $< to $@
|
||||
$(CC) -S $(CFLAGS) $(CONLYFLAGS) $< -o $@
|
||||
|
||||
# Generate Doxygen documents
|
||||
docs:
|
||||
doxygen $(DOXYGENDIR)/doxygen.cfg
|
||||
|
||||
# Target: clean project.
|
||||
clean: begin clean_list finished end
|
||||
|
||||
clean_list :
|
||||
## @echo
|
||||
@echo $(MSG_CLEANING)
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).map
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).elf
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).hex
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).bin
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).sym
|
||||
$(REMOVE) $(OUTDIR)/$(TARGET).lss
|
||||
$(REMOVE) $(ALLOBJ)
|
||||
$(REMOVE) $(LSTFILES)
|
||||
$(REMOVE) $(DEPFILES)
|
||||
$(REMOVE) $(SRC:.c=.s)
|
||||
$(REMOVE) $(SRCARM:.c=.s)
|
||||
$(REMOVE) $(CPPSRC:.cpp=.s)
|
||||
$(REMOVE) $(CPPSRCARM:.cpp=.s)
|
||||
|
||||
|
||||
# Create output files directory
|
||||
# all known MS Windows OS define the ComSpec environment variable
|
||||
ifdef ComSpec
|
||||
$(shell md $(OUTDIR) 2>NUL)
|
||||
else
|
||||
$(shell mkdir $(OUTDIR) 2>/dev/null)
|
||||
endif
|
||||
|
||||
# Include the dependency files.
|
||||
ifdef ComSpec
|
||||
-include $(shell md $(OUTDIR)\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
|
||||
else
|
||||
-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
|
||||
endif
|
||||
|
||||
|
||||
|
||||
# Listing of phony targets.
|
||||
.PHONY : all begin finish end sizebefore sizeafter gccversion \
|
||||
build elf hex bin lss sym clean clean_list program
|
||||
|
@ -1,182 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_comms.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input object: AttitudeSettings
|
||||
* Output object: AttitudeActual
|
||||
*
|
||||
* This module will periodically update the value of latest attitude solution
|
||||
* that is available from the AHRS.
|
||||
* The module settings can configure how often AHRS is polled for a new solution.
|
||||
*
|
||||
* The module executes in its own thread.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "ahrs_comms.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "attitudesettings.h"
|
||||
#include "headingactual.h"
|
||||
#include "ahrsstatus.h"
|
||||
#include "alarms.h"
|
||||
|
||||
#include "pios_opahrs.h" // library for OpenPilot AHRS access functions
|
||||
#include "pios_opahrs_proto.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE 400
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void ahrscommsTask(void* parameters);
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
|
||||
static void update_heading_actual(struct opahrs_msg_v1_rsp_heading * heading);
|
||||
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AHRSCommsInitialize(void)
|
||||
{
|
||||
PIOS_OPAHRS_Init();
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void ahrscommsTask(void* parameters)
|
||||
{
|
||||
// Main task loop
|
||||
while (1) {
|
||||
struct opahrs_msg_v1 rsp;
|
||||
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
|
||||
/* Spin here until we're in sync */
|
||||
while (PIOS_OPAHRS_resync() != OPAHRS_RESULT_OK) {
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
}
|
||||
|
||||
/* Give the other side a chance to keep up */
|
||||
//vTaskDelay(100 / portTICK_RATE_MS);
|
||||
|
||||
if (PIOS_OPAHRS_GetSerial(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_ahrs_status(&(rsp.payload.user.v.rsp.serial));
|
||||
} else {
|
||||
/* Comms error */
|
||||
continue;
|
||||
}
|
||||
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS);
|
||||
|
||||
/* We're in sync with the AHRS, spin here until an error occurs */
|
||||
while (1) {
|
||||
AttitudeSettingsData settings;
|
||||
|
||||
/* Update settings with latest value */
|
||||
AttitudeSettingsGet(&settings);
|
||||
|
||||
if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
|
||||
} else {
|
||||
/* Comms error */
|
||||
break;
|
||||
}
|
||||
|
||||
if (PIOS_OPAHRS_GetHeading(&rsp) == OPAHRS_RESULT_OK) {
|
||||
update_heading_actual(&(rsp.payload.user.v.rsp.heading));
|
||||
} else {
|
||||
/* Comms error */
|
||||
break;
|
||||
}
|
||||
|
||||
/* Wait for the next update interval */
|
||||
vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
|
||||
{
|
||||
AttitudeActualData data;
|
||||
|
||||
data.q1 = attitude->quaternion.q1;
|
||||
data.q2 = attitude->quaternion.q2;
|
||||
data.q3 = attitude->quaternion.q3;
|
||||
data.q4 = attitude->quaternion.q4;
|
||||
|
||||
data.Roll = attitude->euler.roll;
|
||||
data.Pitch = attitude->euler.pitch;
|
||||
data.Yaw = attitude->euler.yaw;
|
||||
|
||||
AttitudeActualSet(&data);
|
||||
}
|
||||
|
||||
static void update_heading_actual(struct opahrs_msg_v1_rsp_heading * heading)
|
||||
{
|
||||
HeadingActualData data;
|
||||
|
||||
data.raw[HEADINGACTUAL_RAW_X] = heading->raw_mag.x;
|
||||
data.raw[HEADINGACTUAL_RAW_Y] = heading->raw_mag.y;
|
||||
data.raw[HEADINGACTUAL_RAW_Z] = heading->raw_mag.z;
|
||||
|
||||
data.heading = heading->heading;
|
||||
|
||||
HeadingActualSet(&data);
|
||||
}
|
||||
|
||||
static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial)
|
||||
{
|
||||
AhrsStatusData data;
|
||||
|
||||
// Get the current object data
|
||||
AhrsStatusGet(&data);
|
||||
|
||||
for (uint8_t i = 0; i < sizeof(serial->serial_bcd); i++) {
|
||||
data.SerialNumber[i] = serial->serial_bcd[i];
|
||||
}
|
||||
|
||||
AhrsStatusSet(&data);
|
||||
}
|
@ -1,34 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrs_comms.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to handle all comms to the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef AHRS_COMMS_H
|
||||
#define AHRS_COMMS_H
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
int32_t AHRSCommsInitialize(void);
|
||||
|
||||
#endif // AHRS_COMMS_H
|
||||
|
@ -1,277 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuator.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Actuator module. Drives the actuators (servos, motors etc).
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "actuator.h"
|
||||
#include "actuatorsettings.h"
|
||||
#include "systemsettings.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "actuatorcommand.h"
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 2
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define FAILSAFE_TIMEOUT_MS 100
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xQueueHandle queue;
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void actuatorTask(void* parameters);
|
||||
static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
|
||||
static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
|
||||
static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd);
|
||||
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral);
|
||||
static void setFailsafe();
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t ActuatorInitialize()
|
||||
{
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for ExampleObject1 updates
|
||||
ActuatorDesiredConnectQueue(queue);
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Main module task
|
||||
*/
|
||||
static void actuatorTask(void* parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
ActuatorSettingsData settings;
|
||||
SystemSettingsData sysSettings;
|
||||
ActuatorDesiredData desired;
|
||||
ActuatorCommandData cmd;
|
||||
|
||||
// Set servo update frequency (done only on start-up)
|
||||
ActuatorSettingsGet(&settings);
|
||||
PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]);
|
||||
|
||||
// Go to the neutral (failsafe) values until an ActuatorDesired update is received
|
||||
setFailsafe();
|
||||
|
||||
// Main task loop
|
||||
while (1)
|
||||
{
|
||||
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
|
||||
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
|
||||
{
|
||||
setFailsafe();
|
||||
continue;
|
||||
}
|
||||
|
||||
// Read settings
|
||||
ActuatorSettingsGet(&settings);
|
||||
SystemSettingsGet(&sysSettings);
|
||||
|
||||
// Reset ActuatorCommand to neutral values
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
cmd.Channel[n] = settings.ChannelNeutral[n];
|
||||
}
|
||||
|
||||
// Read input object
|
||||
ActuatorDesiredGet(&desired);
|
||||
|
||||
// Call appropriate mixer depending on the airframe configuration
|
||||
if ( sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING )
|
||||
{
|
||||
if ( mixerFixedWing(&settings, &desired, &cmd) == -1 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
||||
}
|
||||
}
|
||||
else if ( sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON )
|
||||
{
|
||||
if ( mixerFixedWingElevon(&settings, &desired, &cmd) == -1 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
||||
}
|
||||
}
|
||||
else if ( sysSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )
|
||||
{
|
||||
if ( mixerVTOL(&settings, &desired, &cmd) == -1 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
|
||||
}
|
||||
}
|
||||
|
||||
// Update servo outputs
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
PIOS_Servo_Set( n, cmd.Channel[n] );
|
||||
}
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&cmd);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Mixer for Fixed Wing airframes. Converts desired roll,pitch,yaw and throttle to servo outputs.
|
||||
* @return -1 if error, 0 if success
|
||||
*/
|
||||
static int32_t mixerFixedWing(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd)
|
||||
{
|
||||
// Check settings
|
||||
if ( settings->FixedWingPitch1 == ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE ||
|
||||
settings->FixedWingRoll1 == ACTUATORSETTINGS_FIXEDWINGROLL1_NONE ||
|
||||
settings->FixedWingThrottle == ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set pitch servo command
|
||||
cmd->Channel[ settings->FixedWingPitch1 ] = scaleChannel(desired->Pitch, settings->ChannelMax[ settings->FixedWingPitch1 ],
|
||||
settings->ChannelMin[ settings->FixedWingPitch1 ],
|
||||
settings->ChannelNeutral[ settings->FixedWingPitch1 ]);
|
||||
if ( settings->FixedWingPitch2 != ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE )
|
||||
{
|
||||
cmd->Channel[ settings->FixedWingPitch2 ] = scaleChannel(desired->Pitch, settings->ChannelMax[ settings->FixedWingPitch2 ],
|
||||
settings->ChannelMin[ settings->FixedWingPitch2 ],
|
||||
settings->ChannelNeutral[ settings->FixedWingPitch2 ]);
|
||||
}
|
||||
|
||||
// Set roll servo command
|
||||
cmd->Channel[ settings->FixedWingRoll1 ] = scaleChannel(desired->Roll, settings->ChannelMax[ settings->FixedWingRoll1 ],
|
||||
settings->ChannelMin[ settings->FixedWingRoll1 ],
|
||||
settings->ChannelNeutral[ settings->FixedWingRoll1 ]);
|
||||
if ( settings->FixedWingRoll2 != ACTUATORSETTINGS_FIXEDWINGROLL2_NONE )
|
||||
{
|
||||
cmd->Channel[ settings->FixedWingRoll2 ] = scaleChannel(desired->Roll, settings->ChannelMax[ settings->FixedWingRoll2 ],
|
||||
settings->ChannelMin[ settings->FixedWingRoll2 ],
|
||||
settings->ChannelNeutral[ settings->FixedWingRoll2 ]);
|
||||
}
|
||||
|
||||
// Set yaw servo command
|
||||
if ( settings->FixedWingYaw != ACTUATORSETTINGS_FIXEDWINGYAW_NONE )
|
||||
{
|
||||
cmd->Channel[ settings->FixedWingYaw ] = scaleChannel(desired->Yaw, settings->ChannelMax[ settings->FixedWingYaw ],
|
||||
settings->ChannelMin[ settings->FixedWingYaw ],
|
||||
settings->ChannelNeutral[ settings->FixedWingYaw ]);
|
||||
}
|
||||
|
||||
// Set throttle servo command
|
||||
cmd->Channel[ settings->FixedWingThrottle ] = scaleChannel(desired->Throttle, settings->ChannelMax[ settings->FixedWingThrottle ],
|
||||
settings->ChannelMin[ settings->FixedWingThrottle ],
|
||||
settings->ChannelNeutral[ settings->FixedWingThrottle ]);
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Mixer for Fixed Wing airframes with elevons. Converts desired roll,pitch,yaw and throttle to servo outputs.
|
||||
* @return -1 if error, 0 if success
|
||||
*/
|
||||
static int32_t mixerFixedWingElevon(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd)
|
||||
{
|
||||
// TODO: Implement elevon mixer
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Mixer for VTOL (quads and octo copters). Converts desired roll,pitch,yaw and throttle to servo outputs.
|
||||
* @return -1 if error, 0 if success
|
||||
*/
|
||||
static int32_t mixerVTOL(const ActuatorSettingsData* settings, const ActuatorDesiredData* desired, ActuatorCommandData* cmd)
|
||||
{
|
||||
// TODO: Implement VTOL mixer
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert channel from -1/+1 to servo pulse duration in microseconds
|
||||
*/
|
||||
static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral)
|
||||
{
|
||||
int16_t valueScaled;
|
||||
// Scale
|
||||
if ( value >= 0.0)
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(max-neutral))) + neutral;
|
||||
}
|
||||
else
|
||||
{
|
||||
valueScaled = (int16_t)(value*((float)(neutral-min))) + neutral;
|
||||
}
|
||||
return valueScaled;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set actuator output to the neutral values (failsafe)
|
||||
*/
|
||||
static void setFailsafe()
|
||||
{
|
||||
ActuatorSettingsData settings;
|
||||
ActuatorCommandData cmd;
|
||||
|
||||
// Read settings
|
||||
ActuatorSettingsGet(&settings);
|
||||
|
||||
// Reset ActuatorCommand to neutral values
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
cmd.Channel[n] = settings.ChannelNeutral[n];
|
||||
}
|
||||
|
||||
// Set alarm
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
|
||||
// Update servo outputs
|
||||
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
PIOS_Servo_Set( n, cmd.Channel[n] );
|
||||
}
|
||||
|
||||
// Update output object
|
||||
ActuatorCommandSet(&cmd);
|
||||
}
|
@ -1,31 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuator.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Actuator module. Drives the actuators (servos, motors etc).
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ACTUATOR_H
|
||||
#define ACTUATOR_H
|
||||
|
||||
int32_t ActuatorInitialize();
|
||||
|
||||
#endif // ACTUATOR_H
|
@ -1,107 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file altitude.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, handles temperature and pressure readings from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Output object: AltitudeActual
|
||||
*
|
||||
* This module will periodically update the value of the AltitudeActual object.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "altitudeactual.h" // object that will be updated by the module
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
#define UPDATE_PERIOD 100
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void altitudeTask(void* parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeInitialize()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(altitudeTask, (signed char*)"Altitude", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void altitudeTask(void* parameters)
|
||||
{
|
||||
AltitudeActualData data;
|
||||
portTickType lastSysTime;
|
||||
|
||||
PIOS_BMP085_Init();
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
// Update the temperature data
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
#if 0
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
|
||||
#else
|
||||
vTaskDelay( 5 / portTICK_RATE_MS );
|
||||
#endif
|
||||
PIOS_BMP085_ReadADC();
|
||||
// Convert from 1/10ths of degC to degC
|
||||
data.Temperature = PIOS_BMP085_GetTemperature() / 10.0;
|
||||
|
||||
// Update the pressure data
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
#if 0
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
|
||||
#else
|
||||
vTaskDelay( 26 / portTICK_RATE_MS );
|
||||
#endif
|
||||
PIOS_BMP085_ReadADC();
|
||||
// Convert from Pa to kPa
|
||||
data.Pressure = PIOS_BMP085_GetPressure() / 1000.0;
|
||||
|
||||
// Compute the current altitude (all pressures in kPa)
|
||||
data.Altitude = 44330.0 * (1.0 - powf((data.Pressure/ (BMP085_P0 / 1000.0)), (1.0/5.255)));
|
||||
|
||||
// Update the AltitudeActual UAVObject
|
||||
AltitudeActualSet(&data);
|
||||
|
||||
// Delay until it is time to read the next sample
|
||||
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file altitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Altitude module, reads temperature and pressure from BMP085
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ALTITUDE_H
|
||||
#define ALTITUDE_H
|
||||
|
||||
int32_t AltitudeInitialize();
|
||||
|
||||
#endif // ALTITUDE_H
|
||||
|
@ -1,118 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitude.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to read the attitude solution from the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Output object: FlightBatteryState
|
||||
*
|
||||
* This module will periodically generate information on the battery state.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
#include "flightbatterystate.h"
|
||||
|
||||
//
|
||||
// Configuration
|
||||
//
|
||||
#define DEBUG_PORT PIOS_COM_TELEM_RF
|
||||
#define STACK_SIZE 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define SAMPLE_PERIOD_MS 500
|
||||
//#define ENABLE_DEBUG_MSG
|
||||
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_MSG(format, ...)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void task(void* parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t BatteryInitialize(void)
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(task, (signed char*)"Battery", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void task(void* parameters)
|
||||
{
|
||||
uint cnt = 0;
|
||||
uint32_t mAs = 0;
|
||||
portTickType lastSysTime;
|
||||
FlightBatteryStateData flightBatteryData;
|
||||
|
||||
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
||||
|
||||
DEBUG_MSG("Battery Started\n");
|
||||
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while(1)
|
||||
{
|
||||
// TODO: Compare with floating point calculations
|
||||
|
||||
uint32_t mV, mA;
|
||||
|
||||
mV = PIOS_ADC_PinGet(2)*1257/100;
|
||||
mA = (PIOS_ADC_PinGet(1)-28)*4871/100;
|
||||
mAs += mA * SAMPLE_PERIOD_MS / 1000; // FIXME: Use real time between samples
|
||||
|
||||
DEBUG_MSG("%03d %06d => %06dmV %06d => %06dmA %06dmAh\n", cnt, PIOS_ADC_PinGet(2), mV, PIOS_ADC_PinGet(1), mA, mAs/3600);
|
||||
cnt++;
|
||||
|
||||
flightBatteryData.Voltage = (float)mV/1000;
|
||||
flightBatteryData.Current = (float)mA/1000;
|
||||
flightBatteryData.ConsumedEnergy = mAs / 3600;
|
||||
FlightBatteryStateSet(&flightBatteryData);
|
||||
|
||||
vTaskDelayUntil(&lastSysTime, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
@ -1,34 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitude.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Module to read the attitude solution from the AHRS on a periodic basis.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef BATTERY_H
|
||||
#define BATTERY_H
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
int32_t BatteryInitialize(void);
|
||||
|
||||
#endif // BATTERY_H
|
||||
|
@ -1,59 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodevent.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
* Event callback version.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: ExampleObject1, ExampleSettings
|
||||
* Output object: ExampleObject2
|
||||
*
|
||||
* This module executes in response to ExampleObject1 updates. When the
|
||||
* module is triggered it will update the data of ExampleObject2.
|
||||
*
|
||||
* No threads are used in this example.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "examplemodevent.h"
|
||||
#include "examplemodperiodic.h"
|
||||
#include "examplemodthread.h"
|
||||
|
||||
|
||||
void ExampleInitialize(void)
|
||||
{
|
||||
ExampleModEventInitialize();
|
||||
ExampleModPeriodicInitialize();
|
||||
ExampleModThreadInitialize();
|
||||
}
|
||||
|
@ -1,129 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodevent.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
* Event callback version.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: ExampleObject1, ExampleSettings
|
||||
* Output object: ExampleObject2
|
||||
*
|
||||
* This module executes in response to ExampleObject1 updates. When the
|
||||
* module is triggered it will update the data of ExampleObject2.
|
||||
*
|
||||
* No threads are used in this example.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "examplemodevent.h"
|
||||
#include "exampleobject1.h" // object the module will listen for updates (input)
|
||||
#include "exampleobject2.h" // object the module will update (output)
|
||||
#include "examplesettings.h" // object holding module settings (input)
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
|
||||
// Private functions
|
||||
static void ObjectUpdatedCb(UAVObjEvent* ev);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup.
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t ExampleModEventInitialize()
|
||||
{
|
||||
// Listen for ExampleObject1 updates, connect a callback function
|
||||
ExampleObject1ConnectCallback(&ObjectUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called each time ExampleObject1 is updated, this could be
|
||||
* a local update or a remote update from the GCS. In this example the module
|
||||
* does not have its own thread, the callbacks are executed from within the
|
||||
* event thread. Because of that the callback execution time must be kept to
|
||||
* a minimum, if the callback needs to wait for I/O or other long operations
|
||||
* then a thread should be used instead (see ExampleModThread.c). This is
|
||||
* important since all callbacks are executed from the same thread, so other
|
||||
* queued events can not be executed until the currently active callback returns.
|
||||
*/
|
||||
static void ObjectUpdatedCb(UAVObjEvent* ev)
|
||||
{
|
||||
ExampleSettingsData settings;
|
||||
ExampleObject1Data data1;
|
||||
ExampleObject2Data data2;
|
||||
int32_t step;
|
||||
|
||||
// Make sure that the object update is for ExampleObject1
|
||||
// This is redundant in this case since this callback will
|
||||
// only be called for a single object, it is however possible
|
||||
// to use the same callback for multiple object updates.
|
||||
if ( ev->obj == ExampleObject1Handle() )
|
||||
{
|
||||
// Update settings with latest value
|
||||
ExampleSettingsGet(&settings);
|
||||
|
||||
// Get the input object
|
||||
ExampleObject1Get(&data1);
|
||||
|
||||
// Determine how to update the output object
|
||||
if ( settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP )
|
||||
{
|
||||
step = settings.StepSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
step = -settings.StepSize;
|
||||
}
|
||||
|
||||
// Update data
|
||||
data2.Field1 = data1.Field1 + step;
|
||||
data2.Field2 = data1.Field2 + step;
|
||||
data2.Field3 = data1.Field3 + step;
|
||||
data2.Field4[0] = data1.Field4[0] + step;
|
||||
data2.Field4[1] = data1.Field4[1] + step;
|
||||
|
||||
// Update the ExampleObject2, after this function is called
|
||||
// notifications to any other modules listening to that object
|
||||
// will be sent and the GCS object will be updated through the
|
||||
// telemetry link. All operations will take place asynchronously
|
||||
// and the following call will return immediately.
|
||||
ExampleObject2Set(&data2);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1,126 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodperiodic.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
* Threaded periodic version.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input object: ExampleSettings
|
||||
* Output object: ExampleObject2
|
||||
*
|
||||
* This module will periodically update the value of the ExampleObject object.
|
||||
* The module settings can configure how the ExampleObject is manipulated.
|
||||
*
|
||||
* The module executes in its own thread in this example.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "examplemodperiodic.h"
|
||||
#include "exampleobject2.h" // object that will be updated by the module
|
||||
#include "examplesettings.h" // object holding module settings
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void exampleTask(void* parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t ExampleModPeriodicInitialize()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(exampleTask, (signed char*)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void exampleTask(void* parameters)
|
||||
{
|
||||
ExampleSettingsData settings;
|
||||
ExampleObject2Data data;
|
||||
int32_t step;
|
||||
portTickType lastSysTime;
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
// Update settings with latest value
|
||||
ExampleSettingsGet(&settings);
|
||||
|
||||
// Get the object data
|
||||
ExampleObject2Get(&data);
|
||||
|
||||
// Determine how to update the data
|
||||
if ( settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP )
|
||||
{
|
||||
step = settings.StepSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
step = -settings.StepSize;
|
||||
}
|
||||
|
||||
// Update the data
|
||||
data.Field1 += step;
|
||||
data.Field2 += step;
|
||||
data.Field3 += step;
|
||||
data.Field4[0] += step;
|
||||
data.Field4[1] += step;
|
||||
|
||||
// Update the ExampleObject, after this function is called
|
||||
// notifications to any other modules listening to that object
|
||||
// will be sent and the GCS object will be updated through the
|
||||
// telemetry link. All operations will take place asynchronously
|
||||
// and the following call will return immediately.
|
||||
ExampleObject2Set(&data);
|
||||
|
||||
// Since this module executes at fixed time intervals, we need to
|
||||
// block the task until it is time for the next update.
|
||||
// The settings field is in ms, to convert to RTOS ticks we need
|
||||
// to divide by portTICK_RATE_MS.
|
||||
vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
@ -1,145 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodthread.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
* Threaded version.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input objects: ExampleObject1, ExampleSettings
|
||||
* Output object: ExampleObject2
|
||||
*
|
||||
* This module executes in response to ExampleObject1 updates. When the
|
||||
* module is triggered it will update the data of ExampleObject2.
|
||||
*
|
||||
* The module executes in its own thread in this example.
|
||||
*
|
||||
* UAVObjects are automatically generated by the UAVObjectGenerator from
|
||||
* the object definition XML file.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "examplemodthread.h"
|
||||
#include "exampleobject1.h" // object the module will listen for updates (input)
|
||||
#include "exampleobject2.h" // object the module will update (output)
|
||||
#include "examplesettings.h" // object holding module settings (input)
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 20
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xQueueHandle queue;
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void exampleTask(void* parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t ExampleModThreadInitialize()
|
||||
{
|
||||
// Create object queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Listen for ExampleObject1 updates
|
||||
ExampleObject1ConnectQueue(queue);
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(exampleTask, (signed char*)"ExampleThread", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
static void exampleTask(void* parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
ExampleSettingsData settings;
|
||||
ExampleObject1Data data1;
|
||||
ExampleObject1Data data2;
|
||||
int32_t step;
|
||||
|
||||
// Main task loop
|
||||
while (1)
|
||||
{
|
||||
// Check the event queue for any object updates. In this case
|
||||
// we are only listening for the ExampleSettings object. However
|
||||
// the module could listen to multiple objects.
|
||||
// Since this object only executes on object updates, the task
|
||||
// will block until an object event is pushed in the queue.
|
||||
while ( xQueueReceive(queue, &ev, portMAX_DELAY) != pdTRUE );
|
||||
|
||||
// Make sure that the object update is for ExampleObject1
|
||||
// This is redundant in this example since we have only
|
||||
// registered a single object in the queue, however
|
||||
// in most practical cases there will be more than one
|
||||
// object connected in the queue.
|
||||
if ( ev.obj == ExampleObject1Handle() )
|
||||
{
|
||||
// Update settings with latest value
|
||||
ExampleSettingsGet(&settings);
|
||||
|
||||
// Get the input object
|
||||
ExampleObject1Get(&data1);
|
||||
|
||||
// Determine how to update the output object
|
||||
if ( settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP )
|
||||
{
|
||||
step = settings.StepSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
step = -settings.StepSize;
|
||||
}
|
||||
|
||||
// Update data
|
||||
data2.Field1 = data1.Field1 + step;
|
||||
data2.Field2 = data1.Field2 + step;
|
||||
data2.Field3 = data1.Field3 + step;
|
||||
data2.Field4[0] = data1.Field4[0] + step;
|
||||
data2.Field4[1] = data1.Field4[1] + step;
|
||||
|
||||
// Update the ExampleObject2, after this function is called
|
||||
// notifications to any other modules listening to that object
|
||||
// will be sent and the GCS object will be updated through the
|
||||
// telemetry link. All operations will take place asynchronously
|
||||
// and the following call will return immediately.
|
||||
ExampleObject2Set(&data2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodevent.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODEVENT_H
|
||||
#define EXAMPLEMODEVENT_H
|
||||
|
||||
int32_t ExampleModEventInitialize();
|
||||
|
||||
#endif // EXAMPLEMODEVENT_H
|
||||
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodperiodic.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODPERIODIC_H
|
||||
#define EXAMPLEMODPERIODIC_H
|
||||
|
||||
int32_t ExampleModPeriodicInitialize();
|
||||
|
||||
#endif // EXAMPLEMODPERIODIC_H
|
||||
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplemodthread.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODTHREAD_H
|
||||
#define EXAMPLEMODTHREAD_H
|
||||
|
||||
int32_t ExampleModThreadInitialize();
|
||||
|
||||
#endif // EXAMPLEMODTHREAD_H
|
||||
|
@ -1,715 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file GPS.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "buffer.h"
|
||||
#include "GPS.h"
|
||||
#include "positionactual.h"
|
||||
|
||||
// constants/macros/typdefs
|
||||
#define NMEA_BUFFERSIZE 128
|
||||
|
||||
// Message Codes
|
||||
#define NMEA_NODATA 0 // No data. Packet not available, bad, or not decoded
|
||||
#define NMEA_GPGGA 1 // Global Positioning System Fix Data
|
||||
#define NMEA_GPVTG 2 // Course over ground and ground speed
|
||||
#define NMEA_GPGLL 3 // Geographic position - latitude/longitude
|
||||
#define NMEA_GPGSV 4 // GPS satellites in view
|
||||
#define NMEA_GPGSA 5 // GPS DOP and active satellites
|
||||
#define NMEA_GPRMC 6 // Recommended minimum specific GPS data
|
||||
#define NMEA_UNKNOWN 0xFF// Packet received but not known
|
||||
|
||||
#define GPS_TIMEOUT_MS 500
|
||||
|
||||
// Debugging
|
||||
|
||||
//#define GPSDEBUG
|
||||
|
||||
#ifdef GPSDEBUG
|
||||
#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
|
||||
#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
|
||||
#define NMEA_DEBUG_VTG ///< define to enable debug of VTG messages
|
||||
#define NMEA_DEBUG_RMC ///< define to enable debug of RMC messages
|
||||
#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages
|
||||
#endif
|
||||
|
||||
// Private functions
|
||||
static void gpsTask(void* parameters);
|
||||
|
||||
// functions
|
||||
char* nmeaGetPacketBuffer(void);
|
||||
char nmeaChecksum(char* gps_buffer);
|
||||
uint8_t nmeaProcess(cBuffer* rxBuffer);
|
||||
void nmeaProcessGPGGA(char* packet);
|
||||
void nmeaProcessGPRMC(char* packet);
|
||||
void nmeaProcessGPVTG(char* packet);
|
||||
void nmeaProcessGPGSA(char* packet);
|
||||
|
||||
// Global variables
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static uint8_t gpsPort;
|
||||
static xTaskHandle gpsTaskHandle;
|
||||
cBuffer gpsRxBuffer;
|
||||
static char gpsRxData[512];
|
||||
char NmeaPacket[NMEA_BUFFERSIZE];
|
||||
static uint32_t numUpdates;
|
||||
static uint32_t numErrors;
|
||||
static uint32_t timeOfLastUpdateMs;
|
||||
|
||||
/**
|
||||
* Initialise the gps module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t GPSInitialize(void)
|
||||
{
|
||||
signed portBASE_TYPE xReturn;
|
||||
|
||||
// Init vars
|
||||
numUpdates = 0;
|
||||
numErrors = 0;
|
||||
timeOfLastUpdateMs = 0;
|
||||
|
||||
// TODO: Get gps settings object
|
||||
gpsPort = PIOS_COM_GPS;
|
||||
|
||||
// Init input buffer size 512
|
||||
bufferInit(&gpsRxBuffer, (unsigned char *)gpsRxData, 512);
|
||||
|
||||
// Start gps task
|
||||
xReturn = xTaskCreate(gpsTask, (signed char*)"GPS", STACK_SIZE, NULL, TASK_PRIORITY, &gpsTaskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* gps task. Processes input buffer. It does not return.
|
||||
*/
|
||||
static void gpsTask(void* parameters)
|
||||
{
|
||||
int32_t gpsRxOverflow=0;
|
||||
char c;
|
||||
portTickType xDelay = 100 / portTICK_RATE_MS;
|
||||
PositionActualData GpsData;
|
||||
uint32_t timeNowMs;
|
||||
|
||||
// Loop forever
|
||||
while(1)
|
||||
{
|
||||
/* This blocks the task until there is something on the buffer */
|
||||
while(PIOS_COM_ReceiveBufferUsed(gpsPort) > 0)
|
||||
{
|
||||
c=PIOS_COM_ReceiveBuffer(gpsPort);
|
||||
if( !bufferAddToEnd(&gpsRxBuffer, c) )
|
||||
{
|
||||
// no space in buffer
|
||||
// count overflow
|
||||
gpsRxOverflow++;
|
||||
break;
|
||||
}
|
||||
nmeaProcess(&gpsRxBuffer);
|
||||
}
|
||||
// Check for GPS timeout
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
if ( (timeNowMs - timeOfLastUpdateMs) > GPS_TIMEOUT_MS )
|
||||
{
|
||||
PositionActualGet(&GpsData);
|
||||
GpsData.Status = POSITIONACTUAL_STATUS_NOGPS;
|
||||
PositionActualSet(&GpsData);
|
||||
}
|
||||
// Block task until next update
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
|
||||
char* nmeaGetPacketBuffer(void)
|
||||
{
|
||||
return NmeaPacket;
|
||||
}
|
||||
|
||||
/**
|
||||
* Prosesses NMEA sentence checksum
|
||||
* \param[in] Buffer for parsed nmea sentence
|
||||
* \return 0 checksum not valid
|
||||
* \return 1 checksum valid
|
||||
*/
|
||||
char nmeaChecksum(char* gps_buffer)
|
||||
{
|
||||
char checksum=0;
|
||||
char checksum_received=0;
|
||||
PositionActualData GpsData;
|
||||
PositionActualGet(&GpsData);
|
||||
|
||||
for(int x=0; x<NMEA_BUFFERSIZE; x++)
|
||||
{
|
||||
if(gps_buffer[x]=='*')
|
||||
{
|
||||
//Parsing received checksum...
|
||||
checksum_received = strtol(&gps_buffer[x+1],NULL,16);
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
//XOR the received data...
|
||||
checksum^=gps_buffer[x];
|
||||
}
|
||||
}
|
||||
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%d=%d\r\n",checksum_received,checksum);
|
||||
if(checksum == checksum_received)
|
||||
{
|
||||
++numUpdates;
|
||||
timeOfLastUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
++numErrors;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Prosesses NMEA sentences
|
||||
* \param[in] cBuffer for prosessed nmea sentences
|
||||
* \return Message code for found packet
|
||||
* \return 0xFF NO packet found
|
||||
*/
|
||||
uint8_t nmeaProcess(cBuffer* rxBuffer)
|
||||
{
|
||||
uint8_t foundpacket = NMEA_NODATA;
|
||||
uint8_t startFlag = FALSE;
|
||||
//u08 data;
|
||||
uint16_t i,j;
|
||||
|
||||
// process the receive buffer
|
||||
// go through buffer looking for packets
|
||||
while(rxBuffer->datalength)
|
||||
{
|
||||
// look for a start of NMEA packet
|
||||
if(bufferGetAtIndex(rxBuffer,0) == '$')
|
||||
{
|
||||
// found start
|
||||
startFlag = TRUE;
|
||||
// when start is found, we leave it intact in the receive buffer
|
||||
// in case the full NMEA string is not completely received. The
|
||||
// start will be detected in the next nmeaProcess iteration.
|
||||
|
||||
// done looking for start
|
||||
break;
|
||||
}
|
||||
else
|
||||
bufferGetFromFront(rxBuffer);
|
||||
}
|
||||
|
||||
// if we detected a start, look for end of packet
|
||||
if(startFlag)
|
||||
{
|
||||
for(i=1; i<(rxBuffer->datalength)-1; i++)
|
||||
{
|
||||
// check for end of NMEA packet <CR><LF>
|
||||
if((bufferGetAtIndex(rxBuffer,i) == '\r') && (bufferGetAtIndex(rxBuffer,i+1) == '\n'))
|
||||
{
|
||||
// have a packet end
|
||||
// dump initial '$'
|
||||
bufferGetFromFront(rxBuffer);
|
||||
// copy packet to NmeaPacket
|
||||
for(j=0; j<(i-1); j++)
|
||||
{
|
||||
// although NMEA strings should be 80 characters or less,
|
||||
// receive buffer errors can generate erroneous packets.
|
||||
// Protect against packet buffer overflow
|
||||
if(j<(NMEA_BUFFERSIZE-1))
|
||||
NmeaPacket[j] = bufferGetFromFront(rxBuffer);
|
||||
else
|
||||
bufferGetFromFront(rxBuffer);
|
||||
}
|
||||
// null terminate it
|
||||
if (j<(NMEA_BUFFERSIZE-1)) {
|
||||
NmeaPacket[j] = 0;
|
||||
} else {
|
||||
NmeaPacket[NMEA_BUFFERSIZE-1] = 0;
|
||||
}
|
||||
// dump <CR><LF> from rxBuffer
|
||||
bufferGetFromFront(rxBuffer);
|
||||
bufferGetFromFront(rxBuffer);
|
||||
//DEBUG
|
||||
#ifdef NMEA_DEBUG_PKT
|
||||
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",NmeaPacket);
|
||||
#endif
|
||||
// found a packet
|
||||
// done with this processing session
|
||||
foundpacket = NMEA_UNKNOWN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(foundpacket)
|
||||
{
|
||||
// check message type and process appropriately
|
||||
if(!strncmp(NmeaPacket, "GPGGA", 5))
|
||||
{
|
||||
// process packet of this type
|
||||
nmeaProcessGPGGA(NmeaPacket);
|
||||
// report packet type
|
||||
foundpacket = NMEA_GPGGA;
|
||||
}
|
||||
else if(!strncmp(NmeaPacket, "GPVTG", 5))
|
||||
{
|
||||
// process packet of this type
|
||||
nmeaProcessGPVTG(NmeaPacket);
|
||||
// report packet type
|
||||
foundpacket = NMEA_GPVTG;
|
||||
}
|
||||
else if(!strncmp(NmeaPacket, "GPGSA", 5))
|
||||
{
|
||||
// process packet of this type
|
||||
nmeaProcessGPGSA(NmeaPacket);
|
||||
// report packet type
|
||||
foundpacket = NMEA_GPGSA;
|
||||
}
|
||||
else if(!strncmp(NmeaPacket, "GPRMC", 5))
|
||||
{
|
||||
// process packet of this type
|
||||
nmeaProcessGPRMC(NmeaPacket);
|
||||
// report packet type
|
||||
foundpacket = NMEA_GPRMC;
|
||||
}
|
||||
}
|
||||
else if(rxBuffer->datalength >= rxBuffer->size)
|
||||
{
|
||||
// if we found no packet, and the buffer is full
|
||||
// we're logjammed, flush entire buffer
|
||||
bufferFlush(rxBuffer);
|
||||
}
|
||||
return foundpacket;
|
||||
}
|
||||
|
||||
/**
|
||||
* Prosesses NMEA GPGGA sentences
|
||||
* \param[in] Buffer for parsed nmea GPGGA sentence
|
||||
*/
|
||||
void nmeaProcessGPGGA(char* packet)
|
||||
{
|
||||
PositionActualData GpsData;
|
||||
|
||||
char *tokens;
|
||||
char *delimiter = ",";
|
||||
char *pEnd;
|
||||
char token_length=0;
|
||||
|
||||
long deg,min,desim;
|
||||
|
||||
#ifdef NMEA_DEBUG_GGA
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
|
||||
#endif
|
||||
|
||||
// start parsing just after "GPGGA,"
|
||||
// attempt to reject empty packets right away
|
||||
if(packet[6]==',' && packet[7]==',')
|
||||
return;
|
||||
|
||||
if(!nmeaChecksum(packet))
|
||||
{
|
||||
// checksum not valid
|
||||
return;
|
||||
}
|
||||
|
||||
PositionActualGet(&GpsData);
|
||||
// tokenizer for nmea sentence
|
||||
//GPGGA header
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// get UTC time [hhmmss.sss]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//strcpy(GpsInfo.TimeOfFix,tokens);
|
||||
|
||||
// next field: latitude
|
||||
// get latitude [ddmm.mmmmm]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
token_length=strlen(tokens);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
min=deg%100;
|
||||
deg=deg/100;
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
if(token_length==10)// 5 desimal output
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==9) // 4 desimal output
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==11) // 6 desimal output OPGPS
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
|
||||
// next field: N/S indicator
|
||||
// correct latitute for N/S
|
||||
tokens = strsep(&packet, delimiter);
|
||||
if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
|
||||
|
||||
// next field: longitude
|
||||
// get longitude [dddmm.mmmmm]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
token_length=strlen(tokens);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
min=deg%100;
|
||||
deg=deg/100;
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
if(token_length==11)// 5 desimal output
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==10) // 4 desimal output
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==12) // 6 desimal output OPGPS
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
// next field: E/W indicator
|
||||
// correct latitute for E/W
|
||||
tokens = strsep(&packet, delimiter);
|
||||
if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
|
||||
|
||||
// next field: position fix status
|
||||
// position fix status
|
||||
// 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS
|
||||
// check for good position fix
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//if((tokens[0] != '0') || (tokens[0] != 0))
|
||||
// GpsData.Updates++;
|
||||
|
||||
// next field: satellites used
|
||||
// get number of satellites used in GPS solution
|
||||
tokens = strsep(&packet, delimiter);
|
||||
GpsData.Satellites=atoi(tokens);
|
||||
|
||||
// next field: HDOP (horizontal dilution of precision)
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: altitude
|
||||
// get altitude (in meters mm.m)
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//reuse variables for alt
|
||||
deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
if(1) // OPGPS 3 decimal
|
||||
GpsData.Altitude=deg+desim/1000.0;
|
||||
else
|
||||
GpsData.Altitude=deg+desim/10.0;
|
||||
|
||||
// next field: altitude units, always 'M'
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: geoid separation
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//reuse variables for geoid separation
|
||||
deg=strtol (tokens,&pEnd,10); // always 0.1m resolution? No
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
if(1) // OPGPS 3 decimal
|
||||
GpsData.GeoidSeparation=deg+desim/1000.0;
|
||||
else
|
||||
GpsData.GeoidSeparation=deg+desim/10.0;
|
||||
// next field: separation units
|
||||
// next field: DGPS age
|
||||
// next field: DGPS station ID
|
||||
// next field: checksum
|
||||
PositionActualSet(&GpsData);
|
||||
}
|
||||
|
||||
/**
|
||||
* Prosesses NMEA GPRMC sentences
|
||||
* \param[in] Buffer for parsed nmea GPRMC sentence
|
||||
*/
|
||||
void nmeaProcessGPRMC(char* packet)
|
||||
{
|
||||
PositionActualData GpsData;
|
||||
|
||||
char *tokens;
|
||||
char *delimiter = ",";
|
||||
char *pEnd;
|
||||
char token_length=0;
|
||||
|
||||
long deg,min,desim;
|
||||
|
||||
#ifdef NMEA_DEBUG_RMC
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
|
||||
#endif
|
||||
|
||||
// start parsing just after "GPRMC,"
|
||||
// attempt to reject empty packets right away
|
||||
if(packet[6]==',' && packet[7]==',')
|
||||
return;
|
||||
|
||||
if(!nmeaChecksum(packet))
|
||||
{
|
||||
// checksum not valid
|
||||
return;
|
||||
}
|
||||
|
||||
PositionActualGet(&GpsData);
|
||||
// tokenizer for nmea sentence
|
||||
//GPRMC header
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// get UTC time [hhmmss.sss]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//strcpy(GpsInfo.TimeOfFix,tokens);
|
||||
// next field: Navigation receiver warning A = OK, V = warning
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: latitude
|
||||
// get latitude [ddmm.mmmmm]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
token_length=strlen(tokens);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
min=deg%100;
|
||||
deg=deg/100;
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
/*if(token_length==10)// 5 desimal output
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==9) // 4 desimal output
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==11) // 6 desimal output OPGPS
|
||||
{
|
||||
GpsData.Latitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
|
||||
}*/
|
||||
|
||||
// next field: N/S indicator
|
||||
// correct latitute for N/S
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//if(tokens[0] == 'S') GpsData.Latitude = -GpsData.Latitude;
|
||||
|
||||
// next field: longitude
|
||||
// get longitude [dddmm.mmmmm]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
token_length=strlen(tokens);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
min=deg%100;
|
||||
deg=deg/100;
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
/*if(token_length==11)// 5 desimal output
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/100000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==10) // 4 desimal output
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/10000.0)/60.0; // to desimal degrees
|
||||
}
|
||||
else if(token_length==12) // 6 desimal output OPGPS
|
||||
{
|
||||
GpsData.Longitude=deg+(min+desim/1000000.0)/60.0; // to desimal degrees
|
||||
}*/
|
||||
// next field: E/W indicator
|
||||
// correct latitute for E/W
|
||||
tokens = strsep(&packet, delimiter);
|
||||
//if(tokens[0] == 'W') GpsData.Longitude = -GpsData.Longitude;
|
||||
|
||||
// next field: speed (knots)
|
||||
// get speed in knots
|
||||
tokens = strsep(&packet, delimiter);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
GpsData.Groundspeed = (deg+(desim/100.0))*0.51444; //OPGPS style to m/s
|
||||
|
||||
// next field: True course
|
||||
// get True course
|
||||
tokens = strsep(&packet, delimiter);
|
||||
deg=strtol (tokens,&pEnd,10);
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
GpsData.Heading = deg+(desim/100.0); //OPGPS style
|
||||
|
||||
// next field: Date of fix
|
||||
// get Date of fix
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: Magnetic variation
|
||||
// next field: E or W
|
||||
// next field: checksum
|
||||
PositionActualSet(&GpsData);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Prosesses NMEA GPVTG sentences
|
||||
* \param[in] Buffer for parsed nmea GPVTG sentence
|
||||
*/
|
||||
void nmeaProcessGPVTG(char* packet)
|
||||
{
|
||||
PositionActualData GpsData;
|
||||
|
||||
char *tokens;
|
||||
char *delimiter = ",";
|
||||
|
||||
#ifdef NMEA_DEBUG_VTG
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
|
||||
#endif
|
||||
|
||||
// start parsing just after "GPVTG,"
|
||||
// attempt to reject empty packets right away
|
||||
if(packet[6]==',' && packet[7]==',')
|
||||
return;
|
||||
|
||||
if(!nmeaChecksum(packet))
|
||||
{
|
||||
// checksum not valid
|
||||
return;
|
||||
}
|
||||
PositionActualGet(&GpsData);
|
||||
// tokenizer for nmea sentence
|
||||
|
||||
//GPVTG header
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// get course (true north ref) in degrees [ddd.dd]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: 'T'
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: course (magnetic north)
|
||||
// get course (magnetic north ref) in degrees [ddd.dd]
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: 'M'
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: speed (knots)
|
||||
// get speed in knots
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: 'N'
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: speed (km/h)
|
||||
// get speed in km/h
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: 'K'
|
||||
// next field: checksum
|
||||
PositionActualSet(&GpsData);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Prosesses NMEA GPGSA sentences
|
||||
* \param[in] Buffer for parsed nmea GPGSA sentence
|
||||
*/
|
||||
void nmeaProcessGPGSA(char* packet)
|
||||
{
|
||||
PositionActualData GpsData;
|
||||
|
||||
char *tokens;
|
||||
char *delimiter = ",";
|
||||
char *pEnd;
|
||||
long value,desim;
|
||||
int mode;
|
||||
|
||||
#ifdef NMEA_DEBUG_GSA
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART,"$%s\r\n",packet);
|
||||
#endif
|
||||
|
||||
// start parsing just after "GPGSA,"
|
||||
// attempt to reject empty packets right away
|
||||
if(packet[6]==',' && packet[7]==',')
|
||||
return;
|
||||
|
||||
if(!nmeaChecksum(packet))
|
||||
{
|
||||
// checksum not valid
|
||||
return;
|
||||
}
|
||||
PositionActualGet(&GpsData);
|
||||
// tokenizer for nmea sentence
|
||||
|
||||
//GPGSA header
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: Mode
|
||||
// Mode: M=Manual, forced to operate in 2D or 3D, A=Automatic, 3D/2D
|
||||
tokens = strsep(&packet, delimiter);
|
||||
// next field: Mode
|
||||
// Mode: 1=Fix not available, 2=2D, 3=3D
|
||||
tokens = strsep(&packet, delimiter);
|
||||
mode = atoi(tokens);
|
||||
if (mode == 1)
|
||||
{
|
||||
GpsData.Status = POSITIONACTUAL_STATUS_NOFIX;
|
||||
}
|
||||
else if (mode == 2)
|
||||
{
|
||||
GpsData.Status = POSITIONACTUAL_STATUS_FIX2D;
|
||||
}
|
||||
else if (mode == 3)
|
||||
{
|
||||
GpsData.Status = POSITIONACTUAL_STATUS_FIX3D;
|
||||
}
|
||||
|
||||
// next field: 3-14 IDs of SVs used in position fix (null for unused fields)
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
tokens = strsep(&packet, delimiter);
|
||||
|
||||
// next field: PDOP
|
||||
tokens = strsep(&packet, delimiter);
|
||||
value=strtol (tokens,&pEnd,10);
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
GpsData.PDOP=value+desim/100.0;
|
||||
// next field: HDOP
|
||||
tokens = strsep(&packet, delimiter);
|
||||
value=strtol (tokens,&pEnd,10);
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
GpsData.HDOP=value+desim/100.0;
|
||||
// next field: VDOP
|
||||
tokens = strsep(&packet, delimiter);
|
||||
value=strtol (tokens,&pEnd,10);
|
||||
desim=strtol (pEnd+1,NULL,10);
|
||||
GpsData.VDOP=value+desim/100.0;
|
||||
// next field: checksum
|
||||
PositionActualSet(&GpsData);
|
||||
|
||||
}
|
||||
|
||||
|
@ -1,139 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file buffer.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief see below
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/*! \file buffer.c \brief Multipurpose byte buffer structure and methods. */
|
||||
//*****************************************************************************
|
||||
//
|
||||
// File Name : 'buffer.c'
|
||||
// Title : Multipurpose byte buffer structure and methods
|
||||
// Author : Pascal Stang - Copyright (C) 2001-2002
|
||||
// Created : 9/23/2001
|
||||
// Revised : 9/23/2001
|
||||
// Version : 1.0
|
||||
// Target MCU : any
|
||||
// Editor Tabs : 4
|
||||
//
|
||||
// This code is distributed under the GNU Public License
|
||||
// which can be found at http://www.gnu.org/licenses/gpl.txt
|
||||
//
|
||||
//*****************************************************************************
|
||||
|
||||
#include "buffer.h"
|
||||
|
||||
// global variables
|
||||
|
||||
// initialization
|
||||
|
||||
void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size)
|
||||
{
|
||||
// set start pointer of the buffer
|
||||
buffer->dataptr = start;
|
||||
buffer->size = size;
|
||||
// initialize index and length
|
||||
buffer->dataindex = 0;
|
||||
buffer->datalength = 0;
|
||||
}
|
||||
|
||||
// access routines
|
||||
unsigned char bufferGetFromFront(cBuffer* buffer)
|
||||
{
|
||||
unsigned char data = 0;
|
||||
|
||||
// check to see if there's data in the buffer
|
||||
if(buffer->datalength)
|
||||
{
|
||||
// get the first character from buffer
|
||||
data = buffer->dataptr[buffer->dataindex];
|
||||
// move index down and decrement length
|
||||
buffer->dataindex++;
|
||||
if(buffer->dataindex >= buffer->size)
|
||||
{
|
||||
buffer->dataindex %= buffer->size;
|
||||
}
|
||||
buffer->datalength--;
|
||||
}
|
||||
// return
|
||||
return data;
|
||||
}
|
||||
|
||||
void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes)
|
||||
{
|
||||
// dump numbytes from the front of the buffer
|
||||
// are we dumping less than the entire buffer?
|
||||
if(numbytes < buffer->datalength)
|
||||
{
|
||||
// move index down by numbytes and decrement length by numbytes
|
||||
buffer->dataindex += numbytes;
|
||||
if(buffer->dataindex >= buffer->size)
|
||||
{
|
||||
buffer->dataindex %= buffer->size;
|
||||
}
|
||||
buffer->datalength -= numbytes;
|
||||
}
|
||||
else
|
||||
{
|
||||
// flush the whole buffer
|
||||
buffer->datalength = 0;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index)
|
||||
{
|
||||
// return character at index in buffer
|
||||
return buffer->dataptr[(buffer->dataindex+index)%(buffer->size)];
|
||||
}
|
||||
|
||||
unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data)
|
||||
{
|
||||
// make sure the buffer has room
|
||||
if(buffer->datalength < buffer->size)
|
||||
{
|
||||
// save data byte at end of buffer
|
||||
buffer->dataptr[(buffer->dataindex + buffer->datalength) % buffer->size] = data;
|
||||
// increment the length
|
||||
buffer->datalength++;
|
||||
// return success
|
||||
return -1;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
unsigned char bufferIsNotFull(cBuffer* buffer)
|
||||
{
|
||||
// check to see if the buffer has room
|
||||
// return true if there is room
|
||||
return (buffer->datalength < buffer->size);
|
||||
}
|
||||
|
||||
void bufferFlush(cBuffer* buffer)
|
||||
{
|
||||
// flush contents of the buffer
|
||||
buffer->datalength = 0;
|
||||
}
|
||||
|
@ -1,34 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file GPS.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the GPS module.
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef GPS_H
|
||||
#define GPS_H
|
||||
|
||||
int32_t GPSInitialize(void);
|
||||
|
||||
#endif // GPS_H
|
@ -1,87 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file buffer.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief see below
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
|
||||
/*! \file buffer.h \brief Multipurpose byte buffer structure and methods. */
|
||||
//*****************************************************************************
|
||||
//
|
||||
// File Name : 'buffer.h'
|
||||
// Title : Multipurpose byte buffer structure and methods
|
||||
// Author : Pascal Stang - Copyright (C) 2001-2002
|
||||
// Created : 9/23/2001
|
||||
// Revised : 11/16/2002
|
||||
// Version : 1.1
|
||||
// Target MCU : any
|
||||
// Editor Tabs : 4
|
||||
//
|
||||
/// \ingroup general
|
||||
/// \defgroup buffer Circular Byte-Buffer Structure and Function Library (buffer.c)
|
||||
/// \code #include "buffer.h" \endcode
|
||||
/// \par Overview
|
||||
/// This byte-buffer structure provides an easy and efficient way to store
|
||||
/// and process a stream of bytes. You can create as many buffers as you
|
||||
/// like (within memory limits), and then use this common set of functions to
|
||||
/// access each buffer. The buffers are designed for FIFO operation (first
|
||||
/// in, first out). This means that the first byte you put in the buffer
|
||||
/// will be the first one you get when you read out the buffer. Supported
|
||||
/// functions include buffer initialize, get byte from front of buffer, add
|
||||
/// byte to end of buffer, check if buffer is full, and flush buffer. The
|
||||
/// buffer uses a circular design so no copying of data is ever necessary.
|
||||
/// This buffer is not dynamically allocated, it has a user-defined fixed
|
||||
/// maximum size. This buffer is used in many places in the avrlib code.
|
||||
//
|
||||
// This code is distributed under the GNU Public License
|
||||
// which can be found at http://www.gnu.org/licenses/gpl.txt
|
||||
//
|
||||
//*****************************************************************************
|
||||
//@{
|
||||
|
||||
#ifndef BUFFER_H
|
||||
#define BUFFER_H
|
||||
|
||||
// structure/typdefs
|
||||
|
||||
//! cBuffer structure
|
||||
typedef struct struct_cBuffer
|
||||
{
|
||||
unsigned char *dataptr; ///< the physical memory address where the buffer is stored
|
||||
unsigned short size; ///< the allocated size of the buffer
|
||||
unsigned short datalength; ///< the length of the data currently in the buffer
|
||||
unsigned short dataindex; ///< the index into the buffer where the data starts
|
||||
} cBuffer;
|
||||
|
||||
// function prototypes
|
||||
|
||||
//! initialize a buffer to start at a given address and have given size
|
||||
void bufferInit(cBuffer* buffer, unsigned char *start, unsigned short size);
|
||||
|
||||
//! get the first byte from the front of the buffer
|
||||
unsigned char bufferGetFromFront(cBuffer* buffer);
|
||||
|
||||
//! dump (discard) the first numbytes from the front of the buffer
|
||||
void bufferDumpFromFront(cBuffer* buffer, unsigned short numbytes);
|
||||
|
||||
//! get a byte at the specified index in the buffer (kind of like array access)
|
||||
// ** note: this does not remove the byte that was read from the buffer
|
||||
unsigned char bufferGetAtIndex(cBuffer* buffer, unsigned short index);
|
||||
|
||||
//! add a byte to the end of the buffer
|
||||
unsigned char bufferAddToEnd(cBuffer* buffer, unsigned char data);
|
||||
|
||||
//! check if the buffer is full/not full (returns non-zero value if not full)
|
||||
unsigned char bufferIsNotFull(cBuffer* buffer);
|
||||
|
||||
//! flush (clear) the contents of the buffer
|
||||
void bufferFlush(cBuffer* buffer);
|
||||
|
||||
#endif
|
||||
//@}
|
@ -1,616 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file MKSerial.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Interfacing with MK via serial port
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "MkSerial.h"
|
||||
|
||||
#include "attitudeactual.h" // object that will be updated by the module
|
||||
#include "positionactual.h"
|
||||
#include "flightbatterystate.h"
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Configuration
|
||||
//
|
||||
#define PORT PIOS_COM_AUX
|
||||
#define DEBUG_PORT PIOS_COM_GPS
|
||||
#define STACK_SIZE 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define MAX_NB_PARS 100
|
||||
//#define ENABLE_DEBUG_MSG
|
||||
//#define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state
|
||||
|
||||
#if PORT == PIOS_COM_AUX
|
||||
#ifndef PIOS_ENABLE_AUX_UART
|
||||
#error "This module cannot be included without the AUX UART enabled"
|
||||
#endif
|
||||
#endif
|
||||
//
|
||||
// Private constants
|
||||
//
|
||||
#define MSGCMD_ANY 0
|
||||
#define MSGCMD_GET_DEBUG 'd'
|
||||
#define MSGCMD_DEBUG 'D'
|
||||
#define MSGCMD_GET_VERSION 'v'
|
||||
#define MSGCMD_VERSION 'V'
|
||||
#define MSGCMD_GET_OSD 'o'
|
||||
#define MSGCMD_OSD 'O'
|
||||
|
||||
|
||||
#define DEBUG_MSG_NICK_IDX (2+2*2)
|
||||
#define DEBUG_MSG_ROLL_IDX (2+3*2)
|
||||
|
||||
#define OSD_MSG_CURRPOS_IDX 1
|
||||
#define OSD_MSG_NB_SATS_IDX 50
|
||||
#define OSD_MSG_BATT_IDX 57
|
||||
#define OSD_MSG_COMPHEADING_IDX 62
|
||||
#define OSD_MSG_NICK_IDX 64
|
||||
#define OSD_MSG_ROLL_IDX 65
|
||||
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_MSG(format, ...)
|
||||
#endif
|
||||
|
||||
|
||||
//
|
||||
// Private types
|
||||
//
|
||||
typedef struct
|
||||
{
|
||||
uint8_t address;
|
||||
uint8_t cmd;
|
||||
uint8_t nbPars;
|
||||
uint8_t pars[MAX_NB_PARS];
|
||||
} MkMsg_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float longitute;
|
||||
float latitude;
|
||||
float altitude;
|
||||
uint8_t status;
|
||||
} GpsPosition_t;
|
||||
|
||||
enum
|
||||
{
|
||||
MK_ADDR_ALL = 0,
|
||||
MK_ADDR_FC = 1,
|
||||
MK_ADDR_NC = 2,
|
||||
MK_ADDR_MAG = 3,
|
||||
};
|
||||
|
||||
//
|
||||
// Private variables
|
||||
//
|
||||
|
||||
//
|
||||
// Private functions
|
||||
//
|
||||
static void OnError(int line);
|
||||
//static void PrintMsg(const MkMsg_t* msg);
|
||||
static int16_t Par2Int16(const MkMsg_t* msg, uint8_t index);
|
||||
static int32_t Par2Int32(const MkMsg_t* msg, uint8_t index);
|
||||
static int8_t Par2Int8(const MkMsg_t* msg, uint8_t index);
|
||||
static void GetGpsPos(const MkMsg_t* msg, uint8_t index, GpsPosition_t* pos);
|
||||
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait);
|
||||
static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait);
|
||||
static void SendMsg(const MkMsg_t* msg);
|
||||
static void SendMsgParNone(uint8_t address, uint8_t cmd);
|
||||
static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0);
|
||||
static void MkSerialTask(void* parameters);
|
||||
|
||||
|
||||
|
||||
|
||||
static void OnError(int line)
|
||||
{
|
||||
DEBUG_MSG("MKProcol error %d\n\r", line);
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
static void PrintMsg(const MkMsg_t* msg)
|
||||
{
|
||||
switch (msg->address)
|
||||
{
|
||||
case MK_ADDR_ALL:
|
||||
DEBUG_MSG("ALL ");
|
||||
break;
|
||||
case MK_ADDR_FC:
|
||||
DEBUG_MSG("FC ");
|
||||
break;
|
||||
case MK_ADDR_NC:
|
||||
DEBUG_MSG("NC ");
|
||||
break;
|
||||
case MK_ADDR_MAG:
|
||||
DEBUG_MSG("MAG ");
|
||||
break;
|
||||
default:
|
||||
DEBUG_MSG("??? ");
|
||||
break;
|
||||
}
|
||||
|
||||
DEBUG_MSG("%c ", msg->cmd);
|
||||
|
||||
for (int i = 0; i < msg->nbPars; i++)
|
||||
{
|
||||
DEBUG_MSG("%02x ", msg->pars[i]);
|
||||
}
|
||||
DEBUG_MSG("\n\r");
|
||||
}
|
||||
#endif
|
||||
|
||||
static int16_t Par2Int16(const MkMsg_t* msg, uint8_t index)
|
||||
{
|
||||
int16_t res;
|
||||
|
||||
res = (int) (msg->pars[index + 1]) * 256 + msg->pars[index];
|
||||
if (res > 0xFFFF / 2)
|
||||
res -= 0xFFFF;
|
||||
return res;
|
||||
}
|
||||
|
||||
static int32_t Par2Int32(const MkMsg_t* msg, uint8_t index)
|
||||
{
|
||||
uint32_t val = 0;
|
||||
|
||||
val = (((int)msg->pars[index])<<0)+(((int)msg->pars[index+1])<<8);
|
||||
val += (((int)msg->pars[index+2])<<16)+((int)msg->pars[index+3]<<24);
|
||||
if (val > 0xFFFFFFFF/2)
|
||||
val -= 0xFFFFFFFF;
|
||||
return (int32_t)val;
|
||||
}
|
||||
|
||||
static int8_t Par2Int8(const MkMsg_t* msg, uint8_t index)
|
||||
{
|
||||
if (msg->pars[index] > 127)
|
||||
return msg->pars[index]-256;
|
||||
else
|
||||
return msg->pars[index];
|
||||
}
|
||||
|
||||
static void GetGpsPos(const MkMsg_t* msg, uint8_t index, GpsPosition_t* pos)
|
||||
{
|
||||
pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7;
|
||||
pos->latitude = (float)Par2Int32(msg, index+4) * (float)1e-7;
|
||||
pos->altitude = (float)Par2Int32(msg, index+8) * (float)1e-3 ;
|
||||
pos->status = msg->pars[index+12];
|
||||
}
|
||||
|
||||
|
||||
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait)
|
||||
{
|
||||
uint8_t nbBytesLeft = nbBytes;
|
||||
xTimeOutType xTimeOut;
|
||||
|
||||
vTaskSetTimeOutState(&xTimeOut);
|
||||
|
||||
// Loop until
|
||||
// - all bytes are received
|
||||
// - \r is seen
|
||||
// - Timeout occurs
|
||||
do
|
||||
{
|
||||
// Check if timeout occured
|
||||
if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait))
|
||||
break;
|
||||
|
||||
// Check if there are some bytes
|
||||
if (PIOS_COM_ReceiveBufferUsed(PORT))
|
||||
{
|
||||
*buf = PIOS_COM_ReceiveBuffer(PORT);
|
||||
|
||||
nbBytesLeft--;
|
||||
if (buf[0] == '\r')
|
||||
break;
|
||||
buf++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Avoid tight loop
|
||||
// FIXME: should be blocking
|
||||
vTaskDelay(5);
|
||||
}
|
||||
} while (nbBytesLeft);
|
||||
|
||||
return nbBytes - nbBytesLeft;
|
||||
}
|
||||
|
||||
static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait)
|
||||
{
|
||||
uint8_t buf[10];
|
||||
uint8_t n;
|
||||
bool done = FALSE;
|
||||
bool error = FALSE;
|
||||
unsigned int checkVal;
|
||||
xTimeOutType xTimeOut;
|
||||
|
||||
|
||||
vTaskSetTimeOutState(&xTimeOut);
|
||||
|
||||
while (!done && !error)
|
||||
{
|
||||
// When we are here, it means we did not encounter the message we are waiting for
|
||||
// Check if we did not timeout yet.
|
||||
|
||||
|
||||
// Wait for start
|
||||
buf[0] = 0;
|
||||
do
|
||||
{
|
||||
if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait))
|
||||
{
|
||||
return FALSE;
|
||||
}
|
||||
WaitForBytes(buf, 1, 100 / portTICK_RATE_MS);
|
||||
} while (buf[0] != '#');
|
||||
|
||||
// Wait for cmd and address
|
||||
if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2)
|
||||
{
|
||||
OnError(__LINE__);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Is this the command we are waiting for?
|
||||
if (cmd == 0 || cmd == buf[1])
|
||||
{
|
||||
// OK follow this message to the end
|
||||
msg->address = buf[0] - 'a';
|
||||
msg->cmd = buf[1];
|
||||
|
||||
checkVal = '#' + buf[0] + buf[1];
|
||||
|
||||
// Parse parameters
|
||||
msg->nbPars = 0;
|
||||
while (!done && !error)
|
||||
{
|
||||
n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS);
|
||||
if (n > 0 && buf[n - 1] == '\r')
|
||||
{
|
||||
n--;
|
||||
// This is the end of the message
|
||||
// Get check bytes
|
||||
if (n >= 2)
|
||||
{
|
||||
unsigned int msgCeckVal;
|
||||
msgCeckVal = (buf[n-1]-'=') + (buf[n-2]-'=')*64;
|
||||
//printf("%x %x\n", msgCeckVal, checkVal&0xFFF);
|
||||
n -= 2;
|
||||
|
||||
if (msgCeckVal == (checkVal & 0xFFF))
|
||||
{
|
||||
done = TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
else if (n == 4)
|
||||
{
|
||||
// Parse parameters
|
||||
int i;
|
||||
for (i = 0; i < 4; i++)
|
||||
{
|
||||
checkVal += buf[i];
|
||||
buf[i] -= '=';
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = (((buf[0]<<2)&0xFF) | ((buf[1] >> 4)));
|
||||
msg->nbPars++;
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = ((buf[1] & 0x0F)<< 4 | (buf[2] >> 2));
|
||||
msg->nbPars++;
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = ((buf[2] & 0x03)<< 6 | buf[3]);
|
||||
msg->nbPars++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (done && !error);
|
||||
}
|
||||
|
||||
static void SendMsg(const MkMsg_t* msg)
|
||||
{
|
||||
uint8_t buf[10];
|
||||
uint16_t checkVal;
|
||||
uint8_t nbParsRemaining;
|
||||
const uint8_t* pPar;
|
||||
|
||||
// Header
|
||||
buf[0] = '#';
|
||||
buf[1] = msg->address + 'a';
|
||||
buf[2] = msg->cmd;
|
||||
PIOS_COM_SendBuffer(PORT, buf, 3);
|
||||
checkVal = (unsigned int) '#' + buf[1] + buf[2];
|
||||
|
||||
// Parameters
|
||||
nbParsRemaining = msg->nbPars;
|
||||
pPar = msg->pars;
|
||||
while (nbParsRemaining)
|
||||
{
|
||||
uint8_t a, b, c;
|
||||
|
||||
a = *pPar;
|
||||
b = 0;
|
||||
c = 0;
|
||||
|
||||
nbParsRemaining--;
|
||||
pPar++;
|
||||
if (nbParsRemaining)
|
||||
{
|
||||
b = *pPar;
|
||||
nbParsRemaining--;
|
||||
pPar++;
|
||||
if (nbParsRemaining)
|
||||
{
|
||||
c = *pPar;
|
||||
nbParsRemaining--;
|
||||
pPar++;
|
||||
}
|
||||
}
|
||||
|
||||
buf[0] = (a >> 2) + '=';
|
||||
buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '=';
|
||||
buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '=';
|
||||
buf[3] = (c & 0x3f) + '=';
|
||||
checkVal += buf[0];
|
||||
checkVal += buf[1];
|
||||
checkVal += buf[2];
|
||||
checkVal += buf[3];
|
||||
|
||||
PIOS_COM_SendBuffer(PORT, buf, 4);
|
||||
}
|
||||
|
||||
checkVal &= 0xFFF;
|
||||
buf[0] = (checkVal / 64) + '=';
|
||||
buf[1] = (checkVal % 64) + '=';
|
||||
buf[2] = '\r';
|
||||
PIOS_COM_SendBuffer(PORT, buf, 3);
|
||||
}
|
||||
|
||||
static void SendMsgParNone(uint8_t address, uint8_t cmd)
|
||||
{
|
||||
MkMsg_t msg;
|
||||
|
||||
msg.address = address;
|
||||
msg.cmd = cmd;
|
||||
msg.nbPars = 0;
|
||||
|
||||
SendMsg(&msg);
|
||||
}
|
||||
|
||||
static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0)
|
||||
{
|
||||
MkMsg_t msg;
|
||||
|
||||
msg.address = address;
|
||||
msg.cmd = cmd;
|
||||
msg.nbPars = 1;
|
||||
msg.pars[0] = par0;
|
||||
|
||||
SendMsg(&msg);
|
||||
}
|
||||
|
||||
|
||||
static uint16_t VersionMsg_GetVersion(const MkMsg_t* msg)
|
||||
{
|
||||
return msg->pars[0] * 100 + msg->pars[1];
|
||||
}
|
||||
|
||||
|
||||
static void DoConnectedToFC(void)
|
||||
{
|
||||
AttitudeActualData attitudeData;
|
||||
MkMsg_t msg;
|
||||
|
||||
DEBUG_MSG("FC\n\r");
|
||||
|
||||
memset(&attitudeData, 0, sizeof(attitudeData));
|
||||
|
||||
// Configure FC for fast reporting of the debug-message
|
||||
SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10);
|
||||
|
||||
while (TRUE)
|
||||
{
|
||||
if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS))
|
||||
{
|
||||
int16_t nick;
|
||||
int16_t roll;
|
||||
|
||||
//PrintMsg(&msg);
|
||||
nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX);
|
||||
roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX);
|
||||
|
||||
DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll);
|
||||
|
||||
attitudeData.seq++;
|
||||
attitudeData.Pitch = -(float)nick/10;
|
||||
attitudeData.Roll = -(float)roll/10;
|
||||
AttitudeActualSet(&attitudeData);
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("TO\n\r");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void DoConnectedToNC(void)
|
||||
{
|
||||
MkMsg_t msg;
|
||||
GpsPosition_t pos;
|
||||
AttitudeActualData attitudeData;
|
||||
PositionActualData positionData;
|
||||
FlightBatteryStateData flightBatteryData;
|
||||
#ifdef GENERATE_BATTERY_INFO
|
||||
uint8_t battStateCnt=0;
|
||||
#endif
|
||||
|
||||
DEBUG_MSG("NC\n\r");
|
||||
|
||||
memset(&attitudeData, 0, sizeof(attitudeData));
|
||||
memset(&positionData, 0, sizeof(positionData));
|
||||
memset(&flightBatteryData, 0, sizeof(flightBatteryData));
|
||||
|
||||
// Configure NC for fast reporting of the osd-message
|
||||
SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10);
|
||||
|
||||
while (TRUE)
|
||||
{
|
||||
if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS))
|
||||
{
|
||||
//PrintMsg(&msg);
|
||||
GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);
|
||||
|
||||
#if 0
|
||||
DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
|
||||
DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg,OSD_MSG_NICK_IDX), Par2Int8(&msg,OSD_MSG_ROLL_IDX));
|
||||
DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, (int)pos.longitute, (int)pos.altitude);
|
||||
#else
|
||||
DEBUG_MSG(".");
|
||||
#endif
|
||||
attitudeData.seq++;
|
||||
attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
|
||||
attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
|
||||
AttitudeActualSet(&attitudeData);
|
||||
|
||||
positionData.Longitude = pos.longitute;
|
||||
positionData.Latitude = pos.latitude;
|
||||
positionData.Altitude = pos.altitude;
|
||||
positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX];
|
||||
positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX);
|
||||
if (positionData.Satellites<5)
|
||||
{
|
||||
positionData.Status = POSITIONACTUAL_STATUS_NOFIX;
|
||||
}
|
||||
else
|
||||
{
|
||||
positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
|
||||
}
|
||||
PositionActualSet(&positionData);
|
||||
|
||||
#if GENERATE_BATTERY_INFO
|
||||
if (++battStateCnt > 2)
|
||||
{
|
||||
flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX]/10;
|
||||
FlightBatteryStateSet(&flightBatteryData);
|
||||
battStateCnt = 0;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("TO\n\r");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void MkSerialTask(void* parameters)
|
||||
{
|
||||
MkMsg_t msg;
|
||||
uint32_t version;
|
||||
bool connectionOk = FALSE;
|
||||
|
||||
PIOS_COM_ChangeBaud(PORT, 57600);
|
||||
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
||||
|
||||
DEBUG_MSG("MKSerial Started\n\r");
|
||||
|
||||
while (1)
|
||||
{
|
||||
// Wait until we get version from MK
|
||||
while (!connectionOk)
|
||||
{
|
||||
SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION);
|
||||
DEBUG_MSG("Version... ");
|
||||
if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS))
|
||||
{
|
||||
version = VersionMsg_GetVersion(&msg);
|
||||
DEBUG_MSG("%d\n\r", version);
|
||||
connectionOk = TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("TO\n\r");
|
||||
}
|
||||
}
|
||||
|
||||
// Dependent on version, decide it we are connected to NC or FC
|
||||
if (version < 60)
|
||||
{
|
||||
DoConnectedToNC(); // Will only return after an error
|
||||
}
|
||||
else
|
||||
{
|
||||
DoConnectedToFC(); // Will only return after an error
|
||||
}
|
||||
|
||||
connectionOk = FALSE;
|
||||
|
||||
vTaskDelay(250 / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Initialise the module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t MKSerialInitialize(void)
|
||||
{
|
||||
// Start gps task
|
||||
xTaskCreate(MkSerialTask, (signed char*) "MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
@ -1,34 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file GPS.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the GPS module.
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef MK_SERIAL_H
|
||||
#define MK_SERIAL_H
|
||||
|
||||
int32_t MKSerialInitialize(void);
|
||||
|
||||
#endif // MK_SER_INPUT_H
|
@ -1,31 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrol.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief ManualControl module. Handles safety R/C link and flight mode.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef MANUALCONTROL_H
|
||||
#define MANUALCONTROL_H
|
||||
|
||||
int32_t ManualControlInitialize();
|
||||
|
||||
#endif // MANUALCONTROL_H
|
@ -1,227 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrol.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief ManualControl module. Handles safety R/C link and flight mode.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "manualcontrol.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "attitudedesired.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define UPDATE_PERIOD_MS 20
|
||||
#define THROTTLE_FAILSAFE -0.1
|
||||
#define FLIGHT_MODE_LIMIT 1.0/3.0
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void manualControlTask(void* parameters);
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t ManualControlInitialize()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(manualControlTask, (signed char*)"ManualControl", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module task
|
||||
*/
|
||||
static void manualControlTask(void* parameters)
|
||||
{
|
||||
ManualControlSettingsData settings;
|
||||
StabilizationSettingsData stabSettings;
|
||||
ManualControlCommandData cmd;
|
||||
ActuatorDesiredData actuator;
|
||||
AttitudeDesiredData attitude;
|
||||
portTickType lastSysTime;
|
||||
float flightMode;
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
// Wait until next update
|
||||
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS );
|
||||
|
||||
// Read settings
|
||||
ManualControlSettingsGet(&settings);
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
// Check settings, if error raise alarm
|
||||
if ( settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
|
||||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
|
||||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
|
||||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
|
||||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
||||
ManualControlCommandSet(&cmd);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Read channel values in us
|
||||
// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
|
||||
// selection of PWM and PPM. The configuration is currently done at compile time in
|
||||
// the pios_config.h file.
|
||||
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_PWM)
|
||||
cmd.Channel[n] = PIOS_PWM_Get(n);
|
||||
#elif defined(PIOS_INCLUDE_PPM)
|
||||
cmd.Channel[n] = PIOS_PPM_Get(n);
|
||||
#elif defined(PIOS_INCLUDE_SPEKTRUM)
|
||||
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Calculate roll command in range +1 to -1
|
||||
cmd.Roll = scaleChannel( cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll],
|
||||
settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll] );
|
||||
|
||||
// Calculate pitch command in range +1 to -1
|
||||
cmd.Pitch = scaleChannel( cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch],
|
||||
settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch] );
|
||||
|
||||
// Calculate yaw command in range +1 to -1
|
||||
cmd.Yaw = scaleChannel( cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw],
|
||||
settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw] );
|
||||
|
||||
// Calculate throttle command in range +1 to -1
|
||||
cmd.Throttle = scaleChannel( cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle],
|
||||
settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle] );
|
||||
|
||||
// Update flight mode
|
||||
flightMode = scaleChannel( cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode],
|
||||
settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode] );
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT)
|
||||
{
|
||||
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
|
||||
}
|
||||
else if (flightMode > FLIGHT_MODE_LIMIT)
|
||||
{
|
||||
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
|
||||
}
|
||||
|
||||
// Check for connection status (negative throttle values)
|
||||
// The receiver failsafe for the throttle channel should be set to a value below the channel NEUTRAL
|
||||
if ( cmd.Throttle < THROTTLE_FAILSAFE )
|
||||
{
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
||||
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
else
|
||||
{
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
|
||||
if ( cmd.Throttle < 0 )
|
||||
{
|
||||
cmd.Throttle = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update the ManualControlCommand object
|
||||
ManualControlCommandSet(&cmd);
|
||||
|
||||
// Depending on the mode update the Stabilization or Actuator objects
|
||||
if ( cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
|
||||
{
|
||||
actuator.Roll = cmd.Roll;
|
||||
actuator.Pitch = cmd.Pitch;
|
||||
actuator.Yaw = cmd.Yaw;
|
||||
actuator.Throttle = cmd.Throttle;
|
||||
ActuatorDesiredSet(&actuator);
|
||||
}
|
||||
else if ( cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED )
|
||||
{
|
||||
attitude.Roll = cmd.Roll*stabSettings.RollMax;
|
||||
attitude.Pitch = cmd.Pitch*stabSettings.PitchMax;
|
||||
attitude.Yaw = cmd.Yaw*180.0;
|
||||
attitude.Throttle = cmd.Throttle*stabSettings.ThrottleMax;
|
||||
AttitudeDesiredSet(&attitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
|
||||
*/
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
|
||||
{
|
||||
float valueScaled;
|
||||
// Scale
|
||||
if ( (max > min && value >= neutral) || (min > max && value <= neutral) )
|
||||
{
|
||||
if ( max != neutral )
|
||||
{
|
||||
valueScaled = (float)(value-neutral)/(float)(max-neutral);
|
||||
}
|
||||
else
|
||||
{
|
||||
valueScaled = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( min != neutral )
|
||||
{
|
||||
valueScaled = (float)(value-neutral)/(float)(neutral-min);
|
||||
}
|
||||
else
|
||||
{
|
||||
valueScaled = 0;
|
||||
}
|
||||
}
|
||||
// Bound
|
||||
if ( valueScaled > 1.0 )
|
||||
{
|
||||
valueScaled = 1.0;
|
||||
}
|
||||
else if ( valueScaled < -1.0 )
|
||||
{
|
||||
valueScaled = -1.0;
|
||||
}
|
||||
return valueScaled;
|
||||
}
|
@ -1,907 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file MKSerial.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Interfacing with MK via serial port
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
#include "flightbatterystate.h"
|
||||
#include "positionactual.h"
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Configuration
|
||||
//
|
||||
#define DEBUG_PORT PIOS_COM_TELEM_RF
|
||||
#define STACK_SIZE 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define ENABLE_DEBUG_MSG
|
||||
//#define DUMP_CONFIG // Enable this do read and dump the OSD config
|
||||
//#define WRITE_CONFIG // Enable this do write and verify the OSD config
|
||||
//#define DO_PAR_SEEK // Enable this to start a tool to find where parameters are encoded
|
||||
|
||||
|
||||
//
|
||||
// Private constants
|
||||
//
|
||||
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_MSG(format, ...)
|
||||
#endif
|
||||
|
||||
#define CONFIG_LENGTH 6726
|
||||
#define MIN(a,b) ((a)<(b)?(a):(b))
|
||||
|
||||
#define OSDMSG_V_LS_IDX 10
|
||||
#define OSDMSG_A_LS_IDX 17
|
||||
#define OSDMSG_VA_MS_IDX 18
|
||||
#define OSDMSG_LAT_IDX 33
|
||||
#define OSDMSG_LON_IDX 37
|
||||
#define OSDMSG_HOME_IDX 47
|
||||
#define OSDMSG_ALT_IDX 49
|
||||
#define OSDMSG_NB_SATS 58
|
||||
#define OSDMSG_GPS_STAT 59
|
||||
|
||||
#define OSDMSG_GPS_STAT_NOFIX 0x03
|
||||
#define OSDMSG_GPS_STAT_FIX 0x2B
|
||||
#define OSDMSG_GPS_STAT_HB_FLAG 0x10
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Private types
|
||||
//
|
||||
|
||||
//
|
||||
// Private variables
|
||||
//
|
||||
|
||||
// | Header / cmd?| | V | | V | | LAT: 37 57.0000 | LONG: 24 00.4590 | | Hom<-179| Alt 545.4 m | |#sat|stat|
|
||||
// 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
|
||||
uint8_t msg[63] = {0x03,0x3F,0x03,0x00,0x00,0x00,0x00,0x00,0x90,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFC,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x09,0x60,0x10,0x02,0x00,0x00,0x90,0x00,0x54,0x54,0x00,0x00,0x33,0x28,0x13,0x00,0x00,0x08,0x00,0x00,0x90,0x0A};
|
||||
static volatile bool newPosData=FALSE;
|
||||
static volatile bool newBattData=FALSE;
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Private functions
|
||||
//
|
||||
static void WriteToMsg8(uint8_t index, uint8_t value)
|
||||
{
|
||||
if (value>100)
|
||||
value = 100;
|
||||
|
||||
msg[index] = ((value/10) << 4) + (value%10);
|
||||
}
|
||||
|
||||
static void WriteToMsg16(uint8_t index, uint16_t value)
|
||||
{
|
||||
WriteToMsg8(index, value % 100);
|
||||
WriteToMsg8(index+1, value / 100);
|
||||
}
|
||||
|
||||
static void WriteToMsg24(uint8_t index, uint32_t value)
|
||||
{
|
||||
WriteToMsg16(index, value % 10000);
|
||||
WriteToMsg8(index+2, value / 10000);
|
||||
}
|
||||
|
||||
static void WriteToMsg32(uint8_t index, uint32_t value)
|
||||
{
|
||||
WriteToMsg16(index, value % 10000);
|
||||
WriteToMsg16(index+2, value / 10000);
|
||||
}
|
||||
|
||||
static void SetCoord(uint8_t index, float coord)
|
||||
{
|
||||
uint32_t deg = (uint32_t)coord;
|
||||
float sec = (coord-deg)*60;
|
||||
|
||||
WriteToMsg24(index, sec*10000);
|
||||
WriteToMsg8(index+3, deg);
|
||||
}
|
||||
|
||||
static void SetCourse(uint16_t dir)
|
||||
{
|
||||
WriteToMsg16(OSDMSG_HOME_IDX, dir);
|
||||
}
|
||||
|
||||
static void SetAltitude(uint32_t altitudeMeter)
|
||||
{
|
||||
WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter*10);
|
||||
}
|
||||
|
||||
static void SetVoltage(uint32_t milliVolt)
|
||||
{
|
||||
msg[OSDMSG_VA_MS_IDX] &= 0x0F;
|
||||
msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444)<<4;
|
||||
msg[OSDMSG_V_LS_IDX] = (milliVolt % 6444)*256/6444;
|
||||
}
|
||||
|
||||
static void SetCurrent(uint32_t milliAmp)
|
||||
{
|
||||
uint32_t value = (milliAmp*16570/1000000) + 0x7FA;
|
||||
msg[OSDMSG_VA_MS_IDX] &= 0xF0;
|
||||
msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F);
|
||||
msg[OSDMSG_A_LS_IDX] = (value & 0xFF);
|
||||
}
|
||||
|
||||
|
||||
static void SetNbSats(uint8_t nb)
|
||||
{
|
||||
msg[OSDMSG_NB_SATS] = nb;
|
||||
}
|
||||
|
||||
static void FlightBatteryStateUpdatedCb(UAVObjEvent* ev)
|
||||
{
|
||||
newBattData = TRUE;
|
||||
}
|
||||
|
||||
static void PositionActualUpdatedCb(UAVObjEvent* ev)
|
||||
{
|
||||
newPosData = TRUE;
|
||||
}
|
||||
|
||||
#ifdef DUMP_CONFIG
|
||||
|
||||
static bool Read(uint32_t start, uint8_t length, uint8_t* buffer)
|
||||
{
|
||||
bool res = FALSE;
|
||||
|
||||
if (PIOS_I2C_LockDevice(5000 / portTICK_RATE_MS))
|
||||
{
|
||||
uint8_t cmd[5];
|
||||
|
||||
cmd[0] = 0x02;
|
||||
cmd[1] = 0x05;
|
||||
cmd[2] = (uint8_t)(start & 0xFF);
|
||||
cmd[3] = (uint8_t)(start >> 8);
|
||||
cmd[4] = length;
|
||||
|
||||
PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 0);
|
||||
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 5) == 0)
|
||||
{
|
||||
if (PIOS_I2C_Transfer(I2C_Read, 0x30<<1, buffer, length) == 0)
|
||||
{
|
||||
res = TRUE;
|
||||
}
|
||||
}
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
static void DumpConfig(void)
|
||||
{
|
||||
uint8_t buf[0x78];
|
||||
uint32_t addr=0;
|
||||
uint32_t n=0x78;
|
||||
uint8_t page;
|
||||
bool ok;
|
||||
uint32_t i;
|
||||
|
||||
while (addr<CONFIG_LENGTH)
|
||||
{
|
||||
n = MIN(CONFIG_LENGTH-addr, sizeof(buf));
|
||||
ok = FALSE;
|
||||
while (!ok)
|
||||
{
|
||||
if (Read(addr, n, buf))
|
||||
{
|
||||
#if 0
|
||||
for(uint32_t i=0; i<n; i++)
|
||||
DEBUG_MSG("0x%02x, ", buf[i]);
|
||||
#else
|
||||
page=(addr/0x78)+1;
|
||||
DEBUG_MSG("\rstatic uint8_t addr%02d[] = { ",page);
|
||||
DEBUG_MSG("0x01, 0x7d, 0x%02x, 0x%02x, 0x%02x, ",(uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), n);
|
||||
for(i=0; i<(n-1); i++)
|
||||
DEBUG_MSG("0x%02x, ", buf[i]);
|
||||
DEBUG_MSG("0x%02x };",buf[i]);
|
||||
vTaskDelay(500 / portTICK_RATE_MS);
|
||||
//PIOS_COM_SendBuffer(DEBUG_PORT, buf, n);
|
||||
#endif
|
||||
ok = TRUE;
|
||||
}
|
||||
}
|
||||
DEBUG_MSG("\n\r");
|
||||
addr += n;
|
||||
}
|
||||
|
||||
// {
|
||||
// FILEINFO file;
|
||||
// uint32_t res;
|
||||
//
|
||||
// res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t*)"\\a.txt", DFS_READ, PIOS_SDCARD_Sector, &file);
|
||||
// if (res == DFS_OK)
|
||||
// {
|
||||
// DFS_Close(&file);
|
||||
// }
|
||||
// DEBUG_MSG("ReadFile = 0x%x\n", res);
|
||||
//
|
||||
// res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t*)"\\ConfDump", DFS_WRITE, PIOS_SDCARD_Sector, &file);
|
||||
// if ( res == DFS_OK )
|
||||
// {
|
||||
// uint32_t bytesWritten;
|
||||
// DEBUG_MSG("File Open\n");
|
||||
// res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, 10);
|
||||
//
|
||||
// DEBUG_MSG("Write 0x%x\n", res);
|
||||
// DFS_Close(&file);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// DEBUG_MSG("Error Opening File %x\n", res);
|
||||
// }
|
||||
// }
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef WRITE_CONFIG
|
||||
static uint8_t addr01[] = { 0x01, 0x7d, 0x00, 0x00, 0x78, 0x31, 0x2e, 0x31, 0x35, 0x46, 0xb4, 0x00, 0x02, 0x03, 0x00, 0xff, 0xff, 0xff, 0xff, 0xee, 0x00, 0x00, 0x00, 0x0e, 0x00, 0xfb, 0x07, 0x00, 0x00, 0xff, 0xff, 0x00, 0x0a, 0x0a, 0x05, 0xff, 0xff, 0x0a, 0x0a, 0x00, 0x00, 0x07, 0x00, 0x0b, 0x00, 0x0d, 0x00, 0x0f, 0x00, 0x11, 0x00, 0x13, 0x00, 0x15, 0x00, 0x16, 0x00, 0x17, 0x00, 0x19, 0x00, 0x1a, 0x00, 0x1b, 0x00, 0x1c, 0x00, 0x1d, 0x00, 0x1e, 0x00, 0x1f, 0x00, 0x20, 0x00, 0x21, 0x00, 0x22, 0x00, 0x23, 0x00, 0x24, 0x00, 0x25, 0x00, 0x26, 0x00, 0x00, 0x00, 0x16, 0x00, 0x1f, 0x00, 0x27, 0x00, 0x2d, 0x00, 0x32, 0x00, 0x37, 0x00, 0x3b, 0x00, 0x3f, 0x00, 0x43, 0x00, 0x47, 0x00, 0x4a, 0x00, 0x4e, 0x00, 0x51, 0x00, 0x54, 0x00, 0x57, 0x00, 0x5a, 0x00, 0x5c, 0x00, 0x5f, 0x00 };
|
||||
static uint8_t addr02[] = { 0x01, 0x7d, 0x78, 0x00, 0x78, 0x62, 0x00, 0x64, 0x00, 0x67, 0x00, 0x69, 0x00, 0x6c, 0x00, 0x6e, 0x00, 0x70, 0x00, 0x72, 0x00, 0x75, 0x00, 0x77, 0x00, 0x79, 0x00, 0x7b, 0x00, 0x7d, 0x00, 0x7f, 0x00, 0x81, 0x00, 0x83, 0x00, 0x85, 0x00, 0x87, 0x00, 0x89, 0x00, 0x8a, 0x00, 0x8c, 0x00, 0x8e, 0x00, 0x90, 0x00, 0x92, 0x00, 0x93, 0x00, 0x95, 0x00, 0x97, 0x00, 0x98, 0x00, 0x9a, 0x00, 0x9c, 0x00, 0x9d, 0x00, 0x9f, 0x00, 0xa0, 0x00, 0xa2, 0x00, 0xa4, 0x00, 0xa5, 0x00, 0xa7, 0x00, 0xa8, 0x00, 0xaa, 0x00, 0xab, 0x00, 0xad, 0x00, 0xae, 0x00, 0xaf, 0x00, 0xb1, 0x00, 0xb2, 0x00, 0xb4, 0x00, 0xb5, 0x00, 0xb7, 0x00, 0xb8, 0x00, 0xb9, 0x00, 0xbb, 0x00, 0xbc, 0x00, 0xbd, 0x00, 0xbf, 0x00, 0xc0, 0x00, 0xc1, 0x00, 0xc3, 0x00, 0xc4, 0x00, 0xc5, 0x00, 0xc7, 0x00 };
|
||||
static uint8_t addr03[] = { 0x01, 0x7d, 0xf0, 0x00, 0x78, 0xc8, 0x00, 0xc9, 0x00, 0xca, 0x00, 0xcc, 0x00, 0xcd, 0x00, 0xce, 0x00, 0xcf, 0x00, 0xd0, 0x00, 0xd2, 0x00, 0xd3, 0x00, 0xd4, 0x00, 0xd5, 0x00, 0xd6, 0x00, 0xd8, 0x00, 0xd9, 0x00, 0xda, 0x00, 0xdb, 0x00, 0xdc, 0x00, 0xdd, 0x00, 0xdf, 0x00, 0xe0, 0x00, 0xe1, 0x00, 0xe2, 0x00, 0xe3, 0x00, 0xe4, 0x00, 0xe5, 0x00, 0xe6, 0x00, 0xe7, 0x00, 0xe9, 0x00, 0xea, 0x00, 0xeb, 0x00, 0xec, 0x00, 0xed, 0x00, 0xee, 0x00, 0xef, 0x00, 0xf0, 0x00, 0xf1, 0x00, 0xf2, 0x00, 0xf3, 0x00, 0xf4, 0x00, 0xf5, 0x00, 0xf6, 0x00, 0xf7, 0x00, 0xf8, 0x00, 0xf9, 0x00, 0xfa, 0x00, 0xfb, 0x00, 0xfc, 0x00, 0xfd, 0x00, 0xfe, 0x00, 0xff, 0x00, 0x00, 0x01, 0x01, 0x01, 0x02, 0x01, 0x03, 0x01, 0x04, 0x01, 0x05, 0x01, 0x06, 0x01, 0x07, 0x01, 0x08, 0x01 };
|
||||
static uint8_t addr04[] = { 0x01, 0x7d, 0x68, 0x01, 0x78, 0x09, 0x01, 0x0a, 0x01, 0x0b, 0x01, 0x0c, 0x01, 0x0d, 0x01, 0x0e, 0x01, 0x0f, 0x01, 0x10, 0x01, 0x11, 0x01, 0x12, 0x01, 0x13, 0x01, 0x13, 0x01, 0x14, 0x01, 0x15, 0x01, 0x16, 0x01, 0x17, 0x01, 0x18, 0x01, 0x19, 0x01, 0x1a, 0x01, 0x1b, 0x01, 0x1c, 0x01, 0x1d, 0x01, 0x1d, 0x01, 0x1e, 0x01, 0x1f, 0x01, 0x20, 0x01, 0x21, 0x01, 0x22, 0x01, 0x23, 0x01, 0x24, 0x01, 0x24, 0x01, 0x25, 0x01, 0x26, 0x01, 0x27, 0x01, 0x28, 0x01, 0x29, 0x01, 0x2a, 0x01, 0x2a, 0x01, 0x2b, 0x01, 0x2c, 0x01, 0x2d, 0x01, 0x2e, 0x01, 0x2f, 0x01, 0x2f, 0x01, 0x30, 0x01, 0x31, 0x01, 0x32, 0x01, 0x33, 0x01, 0x34, 0x01, 0x34, 0x01, 0x35, 0x01, 0x36, 0x01, 0x37, 0x01, 0x38, 0x01, 0x39, 0x01, 0x39, 0x01, 0x3a, 0x01, 0x3b, 0x01, 0x3c, 0x01, 0x3d, 0x01 };
|
||||
static uint8_t addr05[] = { 0x01, 0x7d, 0xe0, 0x01, 0x78, 0x3d, 0x01, 0x3e, 0x01, 0x3f, 0x01, 0x40, 0x01, 0x41, 0x01, 0x41, 0x01, 0x42, 0x01, 0x43, 0x01, 0x44, 0x01, 0x44, 0x01, 0x45, 0x01, 0x46, 0x01, 0x47, 0x01, 0x48, 0x01, 0x48, 0x01, 0x49, 0x01, 0x4a, 0x01, 0x4b, 0x01, 0x4b, 0x01, 0x4c, 0x01, 0x4d, 0x01, 0x4e, 0x01, 0x4e, 0x01, 0x4f, 0x01, 0x50, 0x01, 0x51, 0x01, 0x52, 0x01, 0x52, 0x01, 0x53, 0x01, 0x54, 0x01, 0x54, 0x01, 0x55, 0x01, 0x56, 0x01, 0x57, 0x01, 0x57, 0x01, 0x58, 0x01, 0x59, 0x01, 0x5a, 0x01, 0x5a, 0x01, 0x5b, 0x01, 0x5c, 0x01, 0x5d, 0x01, 0x5d, 0x01, 0x5e, 0x01, 0x5f, 0x01, 0x5f, 0x01, 0x60, 0x01, 0x61, 0x01, 0x62, 0x01, 0x62, 0x01, 0x63, 0x01, 0x64, 0x01, 0x64, 0x01, 0x65, 0x01, 0x66, 0x01, 0x67, 0x01, 0x67, 0x01, 0x30, 0x01, 0x31, 0x01, 0x31, 0x01 };
|
||||
static uint8_t addr06[] = { 0x01, 0x7d, 0x58, 0x02, 0x78, 0x33, 0x01, 0x33, 0x01, 0x33, 0x01, 0x34, 0x01, 0x34, 0x01, 0x34, 0x01, 0x36, 0x01, 0x36, 0x01, 0x38, 0x01, 0x38, 0x01, 0x38, 0x01, 0x39, 0x01, 0x39, 0x01, 0x39, 0x01, 0x3b, 0x01, 0x3b, 0x01, 0x3b, 0x01, 0x3d, 0x01, 0x3d, 0x01, 0x3d, 0x01, 0x3e, 0x01, 0x3e, 0x01, 0x40, 0x01, 0x40, 0x01, 0x40, 0x01, 0x41, 0x01, 0x41, 0x01, 0x41, 0x01, 0x43, 0x01, 0x43, 0x01, 0x43, 0x01, 0x45, 0x01, 0x45, 0x01, 0x45, 0x01, 0x46, 0x01, 0x46, 0x01, 0x46, 0x01, 0x48, 0x01, 0x48, 0x01, 0x48, 0x01, 0x49, 0x01, 0x49, 0x01, 0x4b, 0x01, 0x4b, 0x01, 0x4b, 0x01, 0x4d, 0x01, 0x4d, 0x01, 0x4d, 0x01, 0x4e, 0x01, 0x4e, 0x01, 0x4e, 0x01, 0x50, 0x01, 0x50, 0x01, 0x50, 0x01, 0x51, 0x01, 0x51, 0x01, 0x51, 0x01, 0x53, 0x01, 0x53, 0x01, 0x53, 0x01 };
|
||||
static uint8_t addr07[] = { 0x01, 0x7d, 0xd0, 0x02, 0x78, 0x55, 0x01, 0x55, 0x01, 0x55, 0x01, 0x56, 0x01, 0x56, 0x01, 0x56, 0x01, 0x58, 0x01, 0x58, 0x01, 0x58, 0x01, 0x5a, 0x01, 0x5a, 0x01, 0x5a, 0x01, 0x5b, 0x01, 0x5b, 0x01, 0x5b, 0x01, 0x5d, 0x01, 0x5d, 0x01, 0x5d, 0x01, 0x5d, 0x01, 0x5e, 0x01, 0x5e, 0x01, 0x5e, 0x01, 0x60, 0x01, 0x60, 0x01, 0x60, 0x01, 0x62, 0x01, 0x62, 0x01, 0x62, 0x01, 0x63, 0x01, 0x63, 0x01, 0x63, 0x01, 0x65, 0x01, 0x65, 0x01, 0x65, 0x01, 0x66, 0x01, 0x66, 0x01, 0x66, 0x01, 0x68, 0x01, 0x68, 0x01, 0x68, 0x01, 0x68, 0x01, 0x6a, 0x01, 0x6a, 0x01, 0x6a, 0x01, 0x6b, 0x01, 0x6b, 0x01, 0x6b, 0x01, 0x6d, 0x01, 0x6d, 0x01, 0x6d, 0x01, 0x6e, 0x01, 0x6e, 0x01, 0x6e, 0x01, 0x70, 0x01, 0x70, 0x01, 0x70, 0x01, 0x70, 0x01, 0x72, 0x01, 0x72, 0x01, 0x72, 0x01 };
|
||||
static uint8_t addr08[] = { 0x01, 0x7d, 0x48, 0x03, 0x78, 0x73, 0x01, 0x73, 0x01, 0x73, 0x01, 0x75, 0x01, 0x75, 0x01, 0x75, 0x01, 0x75, 0x01, 0x76, 0x01, 0x76, 0x01, 0x76, 0x01, 0x78, 0x01, 0x78, 0x01, 0x78, 0x01, 0x7a, 0x01, 0x7a, 0x01, 0x7a, 0x01, 0x7a, 0x01, 0x7b, 0x01, 0x7b, 0x01, 0x7b, 0x01, 0x7d, 0x01, 0x7d, 0x01, 0x7d, 0x01, 0x7f, 0x01, 0x7f, 0x01, 0x7f, 0x01, 0x7f, 0x01, 0x80, 0x01, 0x80, 0x01, 0x80, 0x01, 0x82, 0x01, 0x82, 0x01, 0x82, 0x01, 0x83, 0x01, 0x83, 0x01, 0x83, 0x01, 0x83, 0x01, 0x85, 0x01, 0x85, 0x01, 0x85, 0x01, 0x87, 0x01, 0x87, 0x01, 0x87, 0x01, 0x87, 0x01, 0x88, 0x01, 0x88, 0x01, 0x88, 0x01, 0x8a, 0x01, 0x8a, 0x01, 0x8a, 0x01, 0x8a, 0x01, 0x8b, 0x01, 0x8b, 0x01, 0x8b, 0x01, 0x8d, 0x01, 0x8d, 0x01, 0x8d, 0x01, 0x8d, 0x01, 0x8f, 0x01, 0x8f, 0x01 };
|
||||
static uint8_t addr09[] = { 0x01, 0x7d, 0xc0, 0x03, 0x78, 0x8f, 0x01, 0x90, 0x01, 0x90, 0x01, 0x90, 0x01, 0x90, 0x01, 0x92, 0x01, 0x92, 0x01, 0x92, 0x01, 0x93, 0x01, 0x93, 0x01, 0x93, 0x01, 0x93, 0x01, 0x95, 0x01, 0x95, 0x01, 0x95, 0x01, 0x97, 0x01, 0x97, 0x01, 0x97, 0x01, 0x97, 0x01, 0x98, 0x01, 0x98, 0x01, 0x98, 0x01, 0x98, 0x01, 0x9a, 0x01, 0x9a, 0x01, 0x9a, 0x01, 0x9b, 0x01, 0x9b, 0x01, 0x9b, 0x01, 0x9b, 0x01, 0x9d, 0x01, 0x9d, 0x01, 0x9d, 0x01, 0x9d, 0x01, 0x9f, 0x01, 0x9f, 0x01, 0x9f, 0x01, 0xa0, 0x01, 0xa0, 0x01, 0xa0, 0x01, 0xa0, 0x01, 0xa2, 0x01, 0xa2, 0x01, 0xa2, 0x01, 0xa2, 0x01, 0xa4, 0x01, 0xa4, 0x01, 0xa4, 0x01, 0xa5, 0x01, 0xa5, 0x01, 0xa5, 0x01, 0xa5, 0x01, 0xa7, 0x01, 0xa7, 0x01, 0xa7, 0x01, 0xa7, 0x01, 0xa8, 0x01, 0xa8, 0x01, 0xa8, 0x01, 0xa8, 0x01 };
|
||||
static uint8_t addr10[] = { 0x01, 0x7d, 0x38, 0x04, 0x78, 0xaa, 0x01, 0xaa, 0x01, 0xaa, 0x01, 0xac, 0x01, 0xac, 0x01, 0xac, 0x01, 0xac, 0x01, 0xad, 0x01, 0xad, 0x01, 0xad, 0x01, 0xad, 0x01, 0xaf, 0x01, 0xaf, 0x01, 0xaf, 0x01, 0xaf, 0x01, 0xb0, 0x01, 0xb0, 0x01, 0xb0, 0x01, 0xb0, 0x01, 0xb2, 0x01, 0xb2, 0x01, 0xb2, 0x01, 0xb4, 0x01, 0xb4, 0x01, 0xb4, 0x01, 0xb4, 0x01, 0xb5, 0x01, 0xb5, 0x01, 0xb5, 0x01, 0xb5, 0x01, 0xb7, 0x01, 0xb7, 0x01, 0xb7, 0x01, 0xb7, 0x01, 0xb8, 0x01, 0xb8, 0x01, 0xb8, 0x01, 0xb8, 0x01, 0xba, 0x01, 0xba, 0x01, 0xba, 0x01, 0xba, 0x01, 0xbc, 0x01, 0xbc, 0x01, 0xbc, 0x01, 0xbc, 0x01, 0xbd, 0x01, 0xbd, 0x01, 0xbd, 0x01, 0xbd, 0x01, 0xbf, 0x01, 0xbf, 0x01, 0xbf, 0x01, 0xbf, 0x01, 0xc1, 0x01, 0xc1, 0x01, 0xc1, 0x01, 0xc1, 0x01, 0xc2, 0x01, 0xc2, 0x01 };
|
||||
static uint8_t addr11[] = { 0x01, 0x7d, 0xb0, 0x04, 0x78, 0xc2, 0x01, 0xc2, 0x01, 0xc4, 0x01, 0xc4, 0x01, 0xc4, 0x01, 0xc4, 0x01, 0xc5, 0x01, 0xc5, 0x01, 0xc5, 0x01, 0xc5, 0x01, 0xc7, 0x01, 0xc7, 0x01, 0xc7, 0x01, 0xc7, 0x01, 0xc9, 0x01, 0xc9, 0x01, 0xc9, 0x01, 0xc9, 0x01, 0xca, 0x01, 0xca, 0x01, 0xca, 0x01, 0xca, 0x01, 0xcc, 0x01, 0xcc, 0x01, 0xcc, 0x01, 0xcc, 0x01, 0xcd, 0x01, 0xcd, 0x01, 0xcd, 0x01, 0xcd, 0x01, 0xcf, 0x01, 0xcf, 0x01, 0xcf, 0x01, 0xcf, 0x01, 0xd1, 0x01, 0xd1, 0x01, 0xd1, 0x01, 0xd1, 0x01, 0xd2, 0x01, 0xd2, 0x01, 0xd2, 0x01, 0xd2, 0x01, 0xd4, 0x01, 0xd4, 0x01, 0xd4, 0x01, 0xd4, 0x01, 0xd5, 0x01, 0xd5, 0x01, 0xd5, 0x01, 0xd5, 0x01, 0xd7, 0x01, 0xd7, 0x01, 0xd7, 0x01, 0xd7, 0x01, 0xd7, 0x01, 0xd9, 0x01, 0xd9, 0x01, 0xd9, 0x01, 0xd9, 0x01, 0xda, 0x01 };
|
||||
static uint8_t addr12[] = { 0x01, 0x7d, 0x28, 0x05, 0x78, 0xda, 0x01, 0xda, 0x01, 0xda, 0x01, 0xdc, 0x01, 0xdc, 0x01, 0xdc, 0x01, 0xdc, 0x01, 0xdd, 0x01, 0xdd, 0x01, 0xdd, 0x01, 0xdd, 0x01, 0xdf, 0x01, 0xdf, 0x01, 0xdf, 0x01, 0xdf, 0x01, 0xdf, 0x01, 0xe1, 0x01, 0xe1, 0x01, 0xe1, 0x01, 0xe1, 0x01, 0xe2, 0x01, 0xe2, 0x01, 0xe2, 0x01, 0xe2, 0x01, 0xe4, 0x01, 0xe4, 0x01, 0xe4, 0x01, 0xe4, 0x01, 0xe6, 0x01, 0xe6, 0x01, 0xe6, 0x01, 0xe6, 0x01, 0xe6, 0x01, 0xe7, 0x01, 0xe7, 0x01, 0xe7, 0x01, 0xe7, 0x01, 0xe9, 0x01, 0xe9, 0x01, 0xe9, 0x01, 0xe9, 0x01, 0xea, 0x01, 0xea, 0x01, 0xea, 0x01, 0xea, 0x01, 0xea, 0x01, 0xec, 0x01, 0xec, 0x01, 0xec, 0x01, 0xec, 0x01, 0xee, 0x01, 0xee, 0x01, 0xee, 0x01, 0xee, 0x01, 0xef, 0x01, 0xef, 0x01, 0xef, 0x01, 0xef, 0x01, 0xef, 0x01, 0xf1, 0x01 };
|
||||
static uint8_t addr13[] = { 0x01, 0x7d, 0xa0, 0x05, 0x78, 0xf1, 0x01, 0xf1, 0x01, 0xf1, 0x01, 0xf2, 0x01, 0xf2, 0x01, 0xf2, 0x01, 0xf2, 0x01, 0xf4, 0x01, 0xf4, 0x01, 0xf4, 0x01, 0xf4, 0x01, 0xf4, 0x01, 0xf6, 0x01, 0xf6, 0x01, 0xf6, 0x01, 0xf6, 0x01, 0xf7, 0x01, 0xf7, 0x01, 0xf7, 0x01, 0xf7, 0x01, 0xf7, 0x01, 0xf9, 0x01, 0xf9, 0x01, 0xf9, 0x01, 0xf9, 0x01, 0xfa, 0x01, 0xfa, 0x01, 0xfa, 0x01, 0xfa, 0x01, 0xfa, 0x01, 0xfc, 0x01, 0xfc, 0x01, 0xfc, 0x01, 0xfc, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xfe, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0xff, 0x01, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x04, 0x02, 0x04, 0x02, 0x04, 0x02, 0x04, 0x02, 0x04, 0x02, 0x06, 0x02, 0x06, 0x02, 0x06, 0x02 };
|
||||
static uint8_t addr14[] = { 0x01, 0x7d, 0x18, 0x06, 0x78, 0x06, 0x02, 0x07, 0x02, 0x07, 0x02, 0x07, 0x02, 0x07, 0x02, 0x07, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x09, 0x02, 0x0b, 0x02, 0x0b, 0x02, 0x0b, 0x02, 0x0b, 0x02, 0x0c, 0x02, 0x0c, 0x02, 0x0c, 0x02, 0x0c, 0x02, 0x0c, 0x02, 0x0e, 0x02, 0x0e, 0x02, 0x0e, 0x02, 0x0e, 0x02, 0x0e, 0x02, 0x0f, 0x02, 0x0f, 0x02, 0x0f, 0x02, 0x0f, 0x02, 0x11, 0x02, 0x11, 0x02, 0x11, 0x02, 0x11, 0x02, 0x11, 0x02, 0x13, 0x02, 0x13, 0x02, 0x13, 0x02, 0x13, 0x02, 0x13, 0x02, 0x14, 0x02, 0x14, 0x02, 0x14, 0x02, 0x14, 0x02, 0x16, 0x02, 0x16, 0x02, 0x16, 0x02, 0x16, 0x02, 0x16, 0x02, 0x17, 0x02, 0x17, 0x02, 0x17, 0x02, 0x17, 0x02, 0x17, 0x02, 0x19, 0x02, 0x19, 0x02, 0x19, 0x02, 0x19, 0x02, 0x1b, 0x02, 0x1b, 0x02, 0x1b, 0x02 };
|
||||
static uint8_t addr15[] = { 0x01, 0x7d, 0x90, 0x06, 0x78, 0x1b, 0x02, 0x1b, 0x02, 0x1c, 0x02, 0x1c, 0x02, 0x1c, 0x02, 0x1c, 0x02, 0x1c, 0x02, 0x1e, 0x02, 0x1e, 0x02, 0x1e, 0x02, 0x1e, 0x02, 0x1e, 0x02, 0x1f, 0x02, 0x1f, 0x02, 0x1f, 0x02, 0x1f, 0x02, 0x1f, 0x02, 0x21, 0x02, 0x21, 0x02, 0x21, 0x02, 0x21, 0x02, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr16[] = { 0x01, 0x7d, 0x08, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr17[] = { 0x01, 0x7d, 0x80, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr18[] = { 0x01, 0x7d, 0xf8, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x4f, 0x00, 0x9e, 0x00, 0xee, 0x00, 0x3f, 0x01, 0x8f, 0x01, 0xe0, 0x01, 0x31, 0x02, 0x82, 0x02, 0xd3, 0x02, 0x25, 0x03, 0x77, 0x03 };
|
||||
static uint8_t addr19[] = { 0x01, 0x7d, 0x70, 0x08, 0x78, 0xca, 0x03, 0x1c, 0x04, 0x6f, 0x04, 0xc2, 0x04, 0x16, 0x05, 0x69, 0x05, 0xbe, 0x05, 0x12, 0x06, 0x67, 0x06, 0xbc, 0x06, 0x11, 0x07, 0x66, 0x07, 0xbc, 0x07, 0x12, 0x08, 0x69, 0x08, 0xc0, 0x08, 0x17, 0x09, 0x6e, 0x09, 0xc6, 0x09, 0x1e, 0x0a, 0x76, 0x0a, 0xcf, 0x0a, 0x28, 0x0b, 0x82, 0x0b, 0xdb, 0x0b, 0x35, 0x0c, 0x90, 0x0c, 0xea, 0x0c, 0x45, 0x0d, 0xa1, 0x0d, 0xfd, 0x0d, 0x59, 0x0e, 0xb5, 0x0e, 0x12, 0x0f, 0x6f, 0x0f, 0xcd, 0x0f, 0x2b, 0x10, 0x89, 0x10, 0xe7, 0x10, 0x46, 0x11, 0xa6, 0x11, 0x06, 0x12, 0x66, 0x12, 0xc6, 0x12, 0x27, 0x13, 0x89, 0x13, 0xea, 0x13, 0x4c, 0x14, 0xaf, 0x14, 0x12, 0x15, 0x75, 0x15, 0xd9, 0x15, 0x3d, 0x16, 0xa2, 0x16, 0x07, 0x17, 0x6c, 0x17, 0xd2, 0x17, 0x38, 0x18, 0x9f, 0x18, 0x06, 0x19 };
|
||||
static uint8_t addr20[] = { 0x01, 0x7d, 0xe8, 0x08, 0x78, 0x6d, 0x19, 0xd6, 0x19, 0x3e, 0x1a, 0xa7, 0x1a, 0x10, 0x1b, 0x7a, 0x1b, 0xe5, 0x1b, 0x4f, 0x1c, 0xbb, 0x1c, 0x26, 0x1d, 0x93, 0x1d, 0xff, 0x1d, 0x6d, 0x1e, 0xda, 0x1e, 0x49, 0x1f, 0xb7, 0x1f, 0x27, 0x20, 0x96, 0x20, 0x07, 0x21, 0x77, 0x21, 0xe9, 0x21, 0x5b, 0x22, 0xcd, 0x22, 0x40, 0x23, 0xb4, 0x23, 0x28, 0x24, 0x9c, 0x24, 0x11, 0x25, 0x87, 0x25, 0xfe, 0x25, 0x75, 0x26, 0xec, 0x26, 0x64, 0x27, 0xdd, 0x27, 0x56, 0x28, 0xd0, 0x28, 0x4b, 0x29, 0xc6, 0x29, 0x42, 0x2a, 0xbe, 0x2a, 0x3b, 0x2b, 0xb9, 0x2b, 0x38, 0x2c, 0xb7, 0x2c, 0x36, 0x2d, 0xb7, 0x2d, 0x38, 0x2e, 0xba, 0x2e, 0x3c, 0x2f, 0xc0, 0x2f, 0x44, 0x30, 0xc8, 0x30, 0x4e, 0x31, 0xd4, 0x31, 0x5b, 0x32, 0xe3, 0x32, 0x6b, 0x33, 0xf4, 0x33, 0x7e, 0x34, 0x09, 0x35 };
|
||||
static uint8_t addr21[] = { 0x01, 0x7d, 0x60, 0x09, 0x78, 0x95, 0x35, 0x21, 0x36, 0xae, 0x36, 0x3d, 0x37, 0xcc, 0x37, 0x5b, 0x38, 0xec, 0x38, 0x7e, 0x39, 0x10, 0x3a, 0xa3, 0x3a, 0x37, 0x3b, 0xcd, 0x3b, 0x63, 0x3c, 0xfa, 0x3c, 0x92, 0x3d, 0x2b, 0x3e, 0xc4, 0x3e, 0x5f, 0x3f, 0xfb, 0x3f, 0x98, 0x40, 0x36, 0x41, 0xd5, 0x41, 0x75, 0x42, 0x16, 0x43, 0xb8, 0x43, 0x5c, 0x44, 0x00, 0x45, 0xa6, 0x45, 0x4c, 0x46, 0xf4, 0x46, 0x9d, 0x47, 0x47, 0x48, 0xf3, 0x48, 0x9f, 0x49, 0x4d, 0x4a, 0xfc, 0x4a, 0xad, 0x4b, 0x5f, 0x4c, 0x12, 0x4d, 0xc6, 0x4d, 0x7c, 0x4e, 0x33, 0x4f, 0xec, 0x4f, 0xa6, 0x50, 0x61, 0x51, 0x1e, 0x52, 0xdc, 0x52, 0x9c, 0x53, 0x5e, 0x54, 0x21, 0x55, 0xe5, 0x55, 0xac, 0x56, 0x74, 0x57, 0x3d, 0x58, 0x08, 0x59, 0xd5, 0x59, 0xa4, 0x5a, 0x75, 0x5b, 0x47, 0x5c, 0x1b, 0x5d };
|
||||
static uint8_t addr22[] = { 0x01, 0x7d, 0xd8, 0x09, 0x78, 0xf1, 0x5d, 0xc9, 0x5e, 0xa3, 0x5f, 0x7f, 0x60, 0x5d, 0x61, 0x3d, 0x62, 0x1f, 0x63, 0x04, 0x64, 0xea, 0x64, 0xd3, 0x65, 0xbe, 0x66, 0xab, 0x67, 0x9b, 0x68, 0x8d, 0x69, 0x81, 0x6a, 0x78, 0x6b, 0x72, 0x6c, 0x6e, 0x6d, 0x6d, 0x6e, 0x6f, 0x6f, 0x73, 0x70, 0x7a, 0x71, 0x84, 0x72, 0x91, 0x73, 0xa2, 0x74, 0xb5, 0x75, 0xcb, 0x76, 0xe5, 0x77, 0x02, 0x79, 0x23, 0x7a, 0x46, 0x7b, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr23[] = { 0x01, 0x7d, 0x50, 0x0a, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr24[] = { 0x01, 0x7d, 0xc8, 0x0a, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr25[] = { 0x01, 0x7d, 0x40, 0x0b, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr26[] = { 0x01, 0x7d, 0xb8, 0x0b, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr27[] = { 0x01, 0x7d, 0x30, 0x0c, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr28[] = { 0x01, 0x7d, 0xa8, 0x0c, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr29[] = { 0x01, 0x7d, 0x20, 0x0d, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x04, 0x00, 0x09, 0x00, 0x0c, 0x00, 0x10, 0x00, 0x13, 0x00, 0x16, 0x00, 0x19, 0x00, 0x1b, 0x00, 0x1e, 0x00, 0x20, 0x00, 0x23, 0x00, 0x25, 0x00, 0x27, 0x00, 0x29, 0x00, 0x2b, 0x00, 0x2d, 0x00, 0x2f, 0x00, 0x31, 0x00, 0x33, 0x00, 0x34, 0x00, 0x36, 0x00, 0x38, 0x00, 0x3a, 0x00, 0x3b, 0x00, 0x3d, 0x00, 0x3f, 0x00, 0x40, 0x00, 0x42, 0x00, 0x43, 0x00, 0x45, 0x00, 0x46, 0x00, 0x48, 0x00, 0x49, 0x00, 0x4b, 0x00, 0x4c, 0x00 };
|
||||
static uint8_t addr30[] = { 0x01, 0x7d, 0x98, 0x0d, 0x78, 0x4d, 0x00, 0x4f, 0x00, 0x50, 0x00, 0x52, 0x00, 0x53, 0x00, 0x55, 0x00, 0x56, 0x00, 0x57, 0x00, 0x59, 0x00, 0x5a, 0x00, 0x5c, 0x00, 0x5d, 0x00, 0x5e, 0x00, 0x60, 0x00, 0x61, 0x00, 0x63, 0x00, 0x64, 0x00, 0x65, 0x00, 0x67, 0x00, 0x68, 0x00, 0x6a, 0x00, 0x6b, 0x00, 0x6c, 0x00, 0x6e, 0x00, 0x6f, 0x00, 0x70, 0x00, 0x72, 0x00, 0x74, 0x00, 0x75, 0x00, 0x77, 0x00, 0x78, 0x00, 0x79, 0x00, 0x7b, 0x00, 0x7d, 0x00, 0x7e, 0x00, 0x80, 0x00, 0x81, 0x00, 0x83, 0x00, 0x85, 0x00, 0x86, 0x00, 0x88, 0x00, 0x8a, 0x00, 0x8b, 0x00, 0x8d, 0x00, 0x8f, 0x00, 0x91, 0x00, 0x93, 0x00, 0x94, 0x00, 0x96, 0x00, 0x99, 0x00, 0x9b, 0x00, 0x9d, 0x00, 0x9f, 0x00, 0xa1, 0x00, 0xa3, 0x00, 0xa5, 0x00, 0xa8, 0x00, 0xaa, 0x00, 0xad, 0x00, 0xaf, 0x00 };
|
||||
static uint8_t addr31[] = { 0x01, 0x7d, 0x10, 0x0e, 0x78, 0xb2, 0x00, 0xb5, 0x00, 0xb8, 0x00, 0xbb, 0x00, 0xbf, 0x00, 0xc2, 0x00, 0xc6, 0x00, 0xc9, 0x00, 0xcd, 0x00, 0xd2, 0x00, 0xd6, 0x00, 0xdb, 0x00, 0xe1, 0x00, 0xe6, 0x00, 0xed, 0x00, 0xf4, 0x00, 0xfc, 0x00, 0x05, 0x01, 0x0f, 0x01, 0x1b, 0x01, 0x2a, 0x01, 0x3d, 0x01, 0x58, 0x01, 0x82, 0x01, 0xba, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr32[] = { 0x01, 0x7d, 0x88, 0x0e, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr33[] = { 0x01, 0x7d, 0x00, 0x0f, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr34[] = { 0x01, 0x7d, 0x78, 0x0f, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr35[] = { 0x01, 0x7d, 0xf0, 0x0f, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr36[] = { 0x01, 0x7d, 0x68, 0x10, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
|
||||
static uint8_t addr37[] = { 0x01, 0x7d, 0xe0, 0x10, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x20, 0x00, 0x28, 0x00, 0x30, 0x00, 0x38, 0x00, 0x40, 0x00, 0x48, 0x00, 0x50, 0x00, 0x57, 0x00, 0x5f, 0x00, 0x67, 0x00, 0x6f, 0x00, 0x77, 0x00, 0x7e, 0x00, 0x86, 0x00, 0x8e, 0x00 };
|
||||
static uint8_t addr38[] = { 0x01, 0x7d, 0x58, 0x11, 0x78, 0x96, 0x00, 0x9d, 0x00, 0xa5, 0x00, 0xad, 0x00, 0xb4, 0x00, 0xbc, 0x00, 0xc4, 0x00, 0xcc, 0x00, 0xd3, 0x00, 0xdb, 0x00, 0xe3, 0x00, 0xeb, 0x00, 0xf2, 0x00, 0xfa, 0x00, 0x02, 0x01, 0x0a, 0x01, 0x12, 0x01, 0x1a, 0x01, 0x22, 0x01, 0x2a, 0x01, 0x32, 0x01, 0x3a, 0x01, 0x42, 0x01, 0x49, 0x01, 0x52, 0x01, 0x5a, 0x01, 0x62, 0x01, 0x6a, 0x01, 0x72, 0x01, 0x7a, 0x01, 0x82, 0x01, 0x8a, 0x01, 0x92, 0x01, 0x9a, 0x01, 0xa2, 0x01, 0xaa, 0x01, 0xb2, 0x01, 0xba, 0x01, 0xc1, 0x01, 0xc9, 0x01, 0xd1, 0x01, 0xd9, 0x01, 0xe1, 0x01, 0xe9, 0x01, 0xf1, 0x01, 0xf9, 0x01, 0x00, 0x02, 0x08, 0x02, 0x10, 0x02, 0x18, 0x02, 0x20, 0x02, 0x27, 0x02, 0x2f, 0x02, 0x37, 0x02, 0x3f, 0x02, 0x46, 0x02, 0x4e, 0x02, 0x56, 0x02, 0x5d, 0x02, 0x65, 0x02 };
|
||||
static uint8_t addr39[] = { 0x01, 0x7d, 0xd0, 0x11, 0x78, 0x6d, 0x02, 0x75, 0x02, 0x7c, 0x02, 0x84, 0x02, 0x8b, 0x02, 0x93, 0x02, 0x9b, 0x02, 0xa2, 0x02, 0xaa, 0x02, 0xb2, 0x02, 0xb9, 0x02, 0xc1, 0x02, 0xc9, 0x02, 0xd0, 0x02, 0xd8, 0x02, 0xdf, 0x02, 0xe7, 0x02, 0xef, 0x02, 0xf6, 0x02, 0xfe, 0x02, 0x05, 0x03, 0x0d, 0x03, 0x14, 0x03, 0x1c, 0x03, 0x24, 0x03, 0x2b, 0x03, 0x33, 0x03, 0x3a, 0x03, 0x42, 0x03, 0x49, 0x03, 0x51, 0x03, 0x58, 0x03, 0x60, 0x03, 0x67, 0x03, 0x6f, 0x03, 0x76, 0x03, 0x7e, 0x03, 0x85, 0x03, 0x8d, 0x03, 0x95, 0x03, 0x9c, 0x03, 0xa4, 0x03, 0xab, 0x03, 0xb3, 0x03, 0xba, 0x03, 0xc2, 0x03, 0xc9, 0x03, 0xd1, 0x03, 0xd8, 0x03, 0xe0, 0x03, 0xe7, 0x03, 0xef, 0x03, 0xf6, 0x03, 0xfe, 0x03, 0x05, 0x04, 0x0d, 0x04, 0x14, 0x04, 0x1c, 0x04, 0x23, 0x04, 0x2b, 0x04 };
|
||||
static uint8_t addr40[] = { 0x01, 0x7d, 0x48, 0x12, 0x78, 0x32, 0x04, 0x3a, 0x04, 0x41, 0x04, 0x49, 0x04, 0x50, 0x04, 0x58, 0x04, 0x5f, 0x04, 0x67, 0x04, 0x6f, 0x04, 0x76, 0x04, 0x7e, 0x04, 0x85, 0x04, 0x8d, 0x04, 0x94, 0x04, 0x9c, 0x04, 0xa3, 0x04, 0xab, 0x04, 0xb3, 0x04, 0xba, 0x04, 0xc2, 0x04, 0xc9, 0x04, 0xd1, 0x04, 0xd9, 0x04, 0xe0, 0x04, 0xe8, 0x04, 0xef, 0x04, 0xf7, 0x04, 0xff, 0x04, 0x06, 0x05, 0x0e, 0x05, 0x16, 0x05, 0x1d, 0x05, 0x25, 0x05, 0x2d, 0x05, 0x34, 0x05, 0x3c, 0x05, 0x44, 0x05, 0x4b, 0x05, 0x53, 0x05, 0x5b, 0x05, 0x63, 0x05, 0x6a, 0x05, 0x72, 0x05, 0x7a, 0x05, 0x81, 0x05, 0x89, 0x05, 0x91, 0x05, 0x99, 0x05, 0xa1, 0x05, 0xa8, 0x05, 0xb0, 0x05, 0xb8, 0x05, 0xc0, 0x05, 0xc8, 0x05, 0xcf, 0x05, 0xd7, 0x05, 0xdf, 0x05, 0xe7, 0x05, 0xef, 0x05, 0xf7, 0x05 };
|
||||
static uint8_t addr41[] = { 0x01, 0x7d, 0xc0, 0x12, 0x78, 0xff, 0x05, 0x06, 0x06, 0x0e, 0x06, 0x16, 0x06, 0x1e, 0x06, 0x26, 0x06, 0x2e, 0x06, 0x36, 0x06, 0x3e, 0x06, 0x46, 0x06, 0x4e, 0x06, 0x56, 0x06, 0x5e, 0x06, 0x66, 0x06, 0x6e, 0x06, 0x76, 0x06, 0x7e, 0x06, 0x86, 0x06, 0x8e, 0x06, 0x96, 0x06, 0x9e, 0x06, 0xa6, 0x06, 0xae, 0x06, 0xb6, 0x06, 0xbe, 0x06, 0xc6, 0x06, 0xce, 0x06, 0xd6, 0x06, 0xdf, 0x06, 0xe7, 0x06, 0xef, 0x06, 0xf7, 0x06, 0xff, 0x06, 0x07, 0x07, 0x10, 0x07, 0x18, 0x07, 0x20, 0x07, 0x28, 0x07, 0x30, 0x07, 0x39, 0x07, 0x41, 0x07, 0x49, 0x07, 0x51, 0x07, 0x5a, 0x07, 0x62, 0x07, 0x6a, 0x07, 0x73, 0x07, 0x7b, 0x07, 0x83, 0x07, 0x8c, 0x07, 0x94, 0x07, 0x9c, 0x07, 0xa5, 0x07, 0xad, 0x07, 0xb5, 0x07, 0xbe, 0x07, 0xc6, 0x07, 0xcf, 0x07, 0xd7, 0x07, 0xe0, 0x07 };
|
||||
static uint8_t addr42[] = { 0x01, 0x7d, 0x38, 0x13, 0x78, 0xe8, 0x07, 0xf1, 0x07, 0xf9, 0x07, 0x02, 0x08, 0x0a, 0x08, 0x13, 0x08, 0x1b, 0x08, 0x24, 0x08, 0x2c, 0x08, 0x35, 0x08, 0x3e, 0x08, 0x46, 0x08, 0x4f, 0x08, 0x58, 0x08, 0x60, 0x08, 0x69, 0x08, 0x72, 0x08, 0x7a, 0x08, 0x83, 0x08, 0x8c, 0x08, 0x95, 0x08, 0x9d, 0x08, 0xa6, 0x08, 0xaf, 0x08, 0xb8, 0x08, 0xc1, 0x08, 0xca, 0x08, 0xd3, 0x08, 0xdc, 0x08, 0xe5, 0x08, 0xee, 0x08, 0xf7, 0x08, 0xff, 0x08, 0x09, 0x09, 0x12, 0x09, 0x1b, 0x09, 0x24, 0x09, 0x2d, 0x09, 0x36, 0x09, 0x3f, 0x09, 0x48, 0x09, 0x51, 0x09, 0x5b, 0x09, 0x64, 0x09, 0x6d, 0x09, 0x76, 0x09, 0x80, 0x09, 0x89, 0x09, 0x92, 0x09, 0x9c, 0x09, 0xa5, 0x09, 0xaf, 0x09, 0xb8, 0x09, 0xc1, 0x09, 0xc8, 0x09, 0xc8, 0x09, 0xc8, 0x09, 0xc8, 0x09, 0xc8, 0x09, 0xc8, 0x09 };
|
||||
static uint8_t addr43[] = { 0x01, 0x7d, 0xb0, 0x13, 0x78, 0xc8, 0x09, 0xc8, 0x09, 0x54, 0x70, 0x41, 0x00, 0x05, 0x40, 0x50, 0x0d, 0x00, 0x00, 0x04, 0x1a, 0xc8, 0x00, 0x00, 0xff, 0x05, 0x06, 0xe8, 0x00, 0x00, 0xff, 0xff, 0xff, 0x54, 0x70, 0x42, 0x00, 0x05, 0x40, 0x50, 0x0d, 0x00, 0x00, 0x04, 0x16, 0xc8, 0x00, 0x00, 0xff, 0x05, 0x08, 0xe8, 0x00, 0x01, 0xff, 0xff, 0xff, 0x54, 0x70, 0x43, 0x00, 0x05, 0x40, 0x50, 0x0d, 0x00, 0x00, 0x04, 0x14, 0xc8, 0x00, 0x00, 0xff, 0x05, 0x0a, 0xe8, 0x00, 0x02, 0xff, 0xff, 0xff, 0x56, 0x20, 0x20, 0x00, 0x0c, 0x15, 0x90, 0x1a, 0x64, 0x00, 0x07, 0x07, 0xe8, 0x03, 0x03, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00, 0x09, 0x0a, 0xff, 0x41, 0x6c, 0x74, 0x00, 0x05, 0x22, 0x58, 0x08, 0x00, 0x13, 0x02, 0x08, 0x88, 0x13, 0x00, 0xff, 0x04, 0x00, 0x0a, 0x00 };
|
||||
static uint8_t addr44[] = { 0x01, 0x7d, 0x28, 0x14, 0x78, 0x00, 0xff, 0xff, 0xff, 0x53, 0x70, 0x64, 0x00, 0x05, 0x32, 0x22, 0x00, 0x18, 0x00, 0x01, 0x0a, 0x64, 0x00, 0x00, 0xff, 0x03, 0x02, 0x05, 0x00, 0x00, 0xff, 0xff, 0xff, 0x52, 0x50, 0x4d, 0x00, 0x24, 0x00, 0xc0, 0xe1, 0xe4, 0x00, 0x03, 0x0c, 0x00, 0x00, 0x00, 0xff, 0x00, 0x14, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x52, 0x70, 0x42, 0x00, 0x04, 0x00, 0x70, 0x38, 0x39, 0x00, 0x03, 0x18, 0x00, 0x00, 0x00, 0xff, 0x01, 0x1e, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x41, 0x6d, 0x70, 0x00, 0x04, 0x00, 0xa0, 0x17, 0x10, 0x27, 0x0c, 0x0e, 0xc8, 0x00, 0x04, 0xff, 0x00, 0x04, 0x64, 0x00, 0x00, 0xff, 0xff, 0xff, 0x57, 0x61, 0x74, 0x00, 0x04, 0x00, 0x00, 0x00, 0xe8, 0x03, 0x23, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x2a, 0x00, 0x00 };
|
||||
static uint8_t addr45[] = { 0x01, 0x7d, 0xa0, 0x14, 0x78, 0x00, 0xff, 0xff, 0xff, 0x50, 0x72, 0x70, 0x00, 0x04, 0x00, 0xe8, 0x03, 0xe8, 0x03, 0x0b, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x24, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x6d, 0x41, 0x48, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe8, 0x03, 0x0d, 0x00, 0xa0, 0x0f, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x41, 0x6d, 0x53, 0x00, 0x04, 0x00, 0x29, 0x00, 0xe8, 0x03, 0x25, 0x1c, 0xc8, 0x00, 0x03, 0xff, 0x01, 0x34, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x54, 0x68, 0x72, 0x00, 0x00, 0x04, 0x64, 0x00, 0xfe, 0x00, 0x08, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x53, 0x76, 0x32, 0x00, 0x00, 0x04, 0x01, 0x00, 0x01, 0x00, 0x08, 0x01, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x80, 0x00 };
|
||||
static uint8_t addr46[] = { 0x01, 0x7d, 0x18, 0x15, 0x78, 0x00, 0xff, 0xff, 0xff, 0x53, 0x76, 0x33, 0x00, 0x00, 0x04, 0x64, 0x00, 0xff, 0x00, 0x08, 0x02, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x80, 0x00, 0x00, 0xff, 0xff, 0xff, 0x53, 0x76, 0x34, 0x00, 0x00, 0x04, 0x64, 0x00, 0xff, 0x00, 0x08, 0x03, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x80, 0x00, 0x00, 0xff, 0xff, 0xff, 0x52, 0x78, 0x56, 0x00, 0x00, 0x04, 0x06, 0xf9, 0x00, 0x00, 0x09, 0x04, 0x00, 0x00, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x47, 0x46, 0x58, 0x00, 0x44, 0x00, 0x64, 0x00, 0x52, 0x00, 0x06, 0x10, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x10, 0xce, 0x0f, 0x00, 0xff, 0xff, 0xff, 0x47, 0x46, 0x59, 0x00, 0x04, 0x00, 0x64, 0x00, 0x57, 0x00, 0x06, 0x12, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x12, 0x56, 0x0f };
|
||||
static uint8_t addr47[] = { 0x01, 0x7d, 0x90, 0x15, 0x78, 0x00, 0xff, 0xff, 0xff, 0x45, 0x47, 0x41, 0x00, 0x05, 0x00, 0x7f, 0x16, 0x00, 0x00, 0x05, 0x14, 0xff, 0xff, 0x00, 0xff, 0x06, 0x0c, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x45, 0x47, 0x42, 0x00, 0x05, 0x00, 0x7f, 0x16, 0x00, 0x00, 0x05, 0x16, 0xff, 0xff, 0x00, 0xff, 0x06, 0x0e, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x42, 0x75, 0x67, 0x00, 0x00, 0x00, 0x90, 0x01, 0xac, 0x01, 0x10, 0x00, 0xff, 0xff, 0x00, 0xff, 0x06, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x43, 0x62, 0x52, 0x00, 0x00, 0x00, 0x58, 0x02, 0x0a, 0x00, 0x0a, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x4e, 0x65, 0x74, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x01, 0x00, 0x12, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr48[] = { 0x01, 0x7d, 0x08, 0x16, 0x78, 0x00, 0xff, 0xff, 0xff, 0x56, 0x61, 0x72, 0x00, 0x00, 0x00, 0x3c, 0x00, 0xd0, 0x07, 0x11, 0x02, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0xc8, 0x00, 0x00, 0xff, 0xff, 0xff, 0x53, 0x61, 0x74, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0xff, 0x53, 0x69, 0x67, 0x00, 0x00, 0x00, 0x64, 0x00, 0x4b, 0x00, 0x0e, 0x00, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x50, 0x6b, 0x25, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x47, 0x50, 0x53, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr49[] = { 0x01, 0x7d, 0x80, 0x16, 0x78, 0x00, 0x00, 0x01, 0xff, 0x53, 0x70, 0x47, 0x00, 0x04, 0x08, 0x7f, 0x04, 0xe8, 0x03, 0x16, 0x26, 0x00, 0x00, 0x00, 0xff, 0x00, 0x26, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x41, 0x6c, 0x74, 0x00, 0x04, 0x09, 0xd1, 0x0c, 0x10, 0x27, 0x17, 0x2e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x09, 0x0b, 0xff, 0x43, 0x6f, 0x75, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x18, 0x2a, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x55, 0x54, 0x43, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x47, 0x53, 0x74, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr50[] = { 0x01, 0x7d, 0xf8, 0x16, 0x78, 0x00, 0x00, 0x00, 0xff, 0x48, 0x44, 0x50, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x2e, 0x00, 0xc8, 0x00, 0x04, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x41, 0x6d, 0x42, 0x00, 0x04, 0x00, 0x6a, 0x02, 0x40, 0x1f, 0x1b, 0x1a, 0xc8, 0x00, 0x04, 0xff, 0x01, 0x16, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x41, 0x6d, 0x43, 0x00, 0x04, 0x00, 0x6a, 0x02, 0xd0, 0x07, 0x1b, 0x10, 0xc8, 0x00, 0x04, 0xff, 0x02, 0x18, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x48, 0x6c, 0x64, 0x00, 0x00, 0x04, 0x01, 0x00, 0x01, 0x00, 0x26, 0x1f, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x4c, 0x73, 0x74, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x27, 0x20, 0x00, 0x00, 0x00, 0xff, 0x00, 0x01, 0x00, 0x00 };
|
||||
static uint8_t addr51[] = { 0x01, 0x7d, 0x70, 0x17, 0x78, 0x00, 0xff, 0xff, 0xff, 0x46, 0x64, 0x41, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x28, 0x22, 0x00, 0x00, 0x00, 0xff, 0x00, 0x02, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x46, 0x64, 0x42, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x28, 0x24, 0x00, 0x00, 0x00, 0xff, 0x00, 0x03, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x46, 0x64, 0x4c, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x28, 0x26, 0x00, 0x00, 0x00, 0xff, 0x00, 0x04, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x46, 0x64, 0x52, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x28, 0x28, 0x00, 0x00, 0x00, 0xff, 0x00, 0x05, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x44, 0x69, 0x73, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x00, 0x2a, 0x00, 0xf4, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
static uint8_t addr52[] = { 0x01, 0x7d, 0xe8, 0x17, 0x78, 0x00, 0x11, 0x0b, 0xff, 0x52, 0x78, 0x48, 0x00, 0x00, 0x04, 0x01, 0x00, 0x01, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x06, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x52, 0x70, 0x43, 0x00, 0x04, 0x00, 0x70, 0x38, 0x39, 0x00, 0x03, 0x1a, 0x00, 0x00, 0x00, 0xff, 0x02, 0x20, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x52, 0x70, 0x44, 0x00, 0x04, 0x00, 0x70, 0x38, 0x39, 0x00, 0x03, 0x1c, 0x00, 0x00, 0x00, 0xff, 0x03, 0x22, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x54, 0x68, 0x25, 0x00, 0x00, 0x04, 0x01, 0x00, 0x02, 0x00, 0x1c, 0x20, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x54, 0x54, 0x70, 0x00, 0x00, 0x04, 0x2e, 0x00, 0x0a, 0x00, 0x1d, 0x25, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x32, 0x00 };
|
||||
static uint8_t addr53[] = { 0x01, 0x7d, 0x60, 0x18, 0x78, 0x00, 0xff, 0xff, 0xff, 0x52, 0x50, 0x4d, 0x00, 0x00, 0x04, 0x00, 0x00, 0x04, 0x00, 0x1e, 0x27, 0xc8, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x45, 0x53, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x50, 0x61, 0x56, 0x00, 0x00, 0x04, 0xe8, 0x03, 0xff, 0x00, 0x20, 0x21, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x50, 0x75, 0x56, 0x00, 0x00, 0x04, 0xe8, 0x03, 0xff, 0x00, 0x21, 0x23, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x56, 0x74, 0x42, 0x00, 0x04, 0x00, 0xa4, 0x00, 0x40, 0x1f, 0x22, 0x1c, 0xc8, 0x00, 0x04, 0xff, 0x02, 0x1a, 0x00, 0x00 };
|
||||
static uint8_t addr54[] = { 0x01, 0x7d, 0xd8, 0x18, 0x78, 0x00, 0xff, 0xff, 0xff, 0x56, 0x74, 0x43, 0x00, 0x04, 0x00, 0xa4, 0x00, 0xd0, 0x07, 0x22, 0x12, 0xc8, 0x00, 0x04, 0xff, 0x02, 0x1c, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x45, 0x47, 0x43, 0x00, 0x05, 0x00, 0x7f, 0x16, 0x00, 0x00, 0x05, 0x10, 0xff, 0xff, 0x00, 0xff, 0x04, 0x30, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x45, 0x47, 0x44, 0x00, 0x05, 0x00, 0x7f, 0x16, 0x00, 0x00, 0x05, 0x12, 0xff, 0xff, 0x00, 0xff, 0x04, 0x32, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x47, 0x46, 0x41, 0x00, 0x04, 0x00, 0x64, 0x00, 0x5c, 0x01, 0x06, 0x16, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x2c, 0x58, 0x3d, 0x00, 0xff, 0xff, 0xff, 0x47, 0x46, 0x42, 0x00, 0x04, 0x00, 0x64, 0x00, 0x5c, 0x01, 0x06, 0x14, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x2e, 0x58, 0x3d };
|
||||
static uint8_t addr55[] = { 0x01, 0x7d, 0x50, 0x19, 0x78, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x41, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x1a, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x42, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x1c, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x43, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x14, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x44, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x16, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x45, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x0a, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10 };
|
||||
static uint8_t addr56[] = { 0x01, 0x7d, 0xc8, 0x19, 0x78, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x46, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x7f, 0x24, 0x08, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x41, 0x64, 0x47, 0x00, 0x00, 0x00, 0xf4, 0x01, 0xff, 0x0f, 0x24, 0x0e, 0xc8, 0x00, 0x03, 0xff, 0x00, 0x00, 0x07, 0x10, 0x00, 0xff, 0xff, 0xff, 0x48, 0x6f, 0x6d, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, 0x0a, 0xff, 0x57, 0x61, 0x79, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xff, 0x4f, 0x50, 0x69, 0x6c, 0x00, 0x00, 0x6f, 0x74, 0x00, 0x00, 0x2b, 0x00, 0x00, 0x00, 0x00, 0xff, 0x00, 0x00, 0x07, 0x00 };
|
||||
static uint8_t addr57[] = { 0x01, 0x7D, 0x40, 0x1A, 0x06, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x46, 0x00, 0x00, 0x00, 0xF4, 0x01, 0xFF, 0x7F, 0x24, 0x08, 0xC8, 0x00, 0x03, 0xFF, 0x00, 0x00, 0x07, 0x10, 0x00, 0xFF, 0xFF, 0xFF, 0x41, 0x64, 0x47, 0x00, 0x00, 0x00, 0xF4, 0x01, 0xFF, 0x0F, 0x24, 0x0E, 0xC8, 0x00, 0x03, 0xFF, 0x00, 0x00, 0x07, 0x10, 0x00, 0xFF, 0xFF, 0xFF, 0x48, 0x6F, 0x6D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x29, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, 0x0A, 0xFF, 0x57, 0x61, 0x79, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0x4F, 0x50, 0x69, 0x6C, 0x00, 0x00, 0x6F, 0x74, 0x00, 0x00, 0x2B, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x07, 0x00};
|
||||
//static uint8_t addr00[] = { 0x01, 0x7D, 0x00, 0x00, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
/*
|
||||
static uint8_t addr01[] = { 0x01, 0x7d, 0x00, 0x00, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x04, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr02[] = { 0x01, 0x7d, 0x78, 0x00, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr03[] = { 0x01, 0x7d, 0xf0, 0x00, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x04, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr04[] = { 0x01, 0x7d, 0x68, 0x01, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr05[] = { 0x01, 0x7d, 0xe0, 0x01, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr06[] = { 0x01, 0x7d, 0x58, 0x02, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr07[] = { 0x01, 0x7d, 0xd0, 0x02, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr08[] = { 0x01, 0x7d, 0x48, 0x03, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x06, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr09[] = { 0x01, 0x7d, 0xc0, 0x03, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x06, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr10[] = { 0x01, 0x7d, 0x38, 0x04, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x06, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr11[] = { 0x01, 0x7d, 0xb0, 0x04, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr12[] = { 0x01, 0x7d, 0x28, 0x05, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr13[] = { 0x01, 0x7d, 0xa0, 0x05, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr14[] = { 0x01, 0x7d, 0x18, 0x06, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x08, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr15[] = { 0x01, 0x7d, 0x90, 0x06, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x08, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr16[] = { 0x01, 0x7d, 0x08, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x08, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr17[] = { 0x01, 0x7d, 0x80, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr18[] = { 0x01, 0x7d, 0xf8, 0x07, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x09, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr19[] = { 0x01, 0x7d, 0x70, 0x08, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x09, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr20[] = { 0x01, 0x7d, 0xe8, 0x08, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0a, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr21[] = { 0x01, 0x7d, 0x60, 0x09, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0a, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr22[] = { 0x01, 0x7d, 0xd8, 0x09, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr23[] = { 0x01, 0x7d, 0x50, 0x0a, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0a, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr24[] = { 0x01, 0x7d, 0xc8, 0x0a, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0b, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr25[] = { 0x01, 0x7d, 0x40, 0x0b, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0b, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr26[] = { 0x01, 0x7d, 0xb8, 0x0b, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0c, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr27[] = { 0x01, 0x7d, 0x30, 0x0c, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr28[] = { 0x01, 0x7d, 0xa8, 0x0c, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0c, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr29[] = { 0x01, 0x7d, 0x20, 0x0d, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0c, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr30[] = { 0x01, 0x7d, 0x98, 0x0d, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0d, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr31[] = { 0x01, 0x7d, 0x10, 0x0e, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0d, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr32[] = { 0x01, 0x7d, 0x88, 0x0e, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr33[] = { 0x01, 0x7d, 0x00, 0x0f, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr34[] = { 0x01, 0x7d, 0x78, 0x0f, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr35[] = { 0x01, 0x7d, 0xf0, 0x0f, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0e, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr36[] = { 0x01, 0x7d, 0x68, 0x10, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr37[] = { 0x01, 0x7d, 0xe0, 0x10, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr38[] = { 0x01, 0x7d, 0x58, 0x11, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr39[] = { 0x01, 0x7d, 0xd0, 0x11, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr40[] = { 0x01, 0x7d, 0x48, 0x12, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x10, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr41[] = { 0x01, 0x7d, 0xc0, 0x12, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x10, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr42[] = { 0x01, 0x7d, 0x38, 0x13, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr43[] = { 0x01, 0x7d, 0xb0, 0x13, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x11, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr44[] = { 0x01, 0x7d, 0x28, 0x14, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x11, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr45[] = { 0x01, 0x7d, 0xa0, 0x14, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x11, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr46[] = { 0x01, 0x7d, 0x18, 0x15, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr47[] = { 0x01, 0x7d, 0x90, 0x15, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr48[] = { 0x01, 0x7d, 0x08, 0x16, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x12, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr49[] = { 0x01, 0x7d, 0x80, 0x16, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x13, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr50[] = { 0x01, 0x7d, 0xf8, 0x16, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x13, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr51[] = { 0x01, 0x7d, 0x70, 0x17, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x13, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr52[] = { 0x01, 0x7d, 0xe8, 0x17, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr53[] = { 0x01, 0x7d, 0x60, 0x18, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x14, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr54[] = { 0x01, 0x7d, 0xd8, 0x18, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x14, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr55[] = { 0x01, 0x7d, 0x50, 0x19, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x15, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr56[] = { 0x01, 0x7d, 0xc8, 0x19, 0x78, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x15, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
static uint8_t addr57[] = { 0x01, 0x7D, 0x40, 0x1A, 0x06, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x15, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
|
||||
*/
|
||||
|
||||
uint8_t WriteConfig(int page)
|
||||
{
|
||||
uint8_t buffer[0x10]={0};
|
||||
uint8_t * p;
|
||||
int32_t error;
|
||||
static uint8_t retry_count=0;
|
||||
if(page==0)
|
||||
return 0;
|
||||
|
||||
if(page==1)
|
||||
p=addr01;
|
||||
if(page==2)
|
||||
p=addr02;
|
||||
if(page==3)
|
||||
p=addr03;
|
||||
if(page==4)
|
||||
p=addr04;
|
||||
if(page==5)
|
||||
p=addr05;
|
||||
if(page==6)
|
||||
p=addr06;
|
||||
if(page==7)
|
||||
p=addr07;
|
||||
if(page==8)
|
||||
p=addr08;
|
||||
if(page==9)
|
||||
p=addr09;
|
||||
if(page==10)
|
||||
p=addr10;
|
||||
if(page==11)
|
||||
p=addr11;
|
||||
if(page==12)
|
||||
p=addr12;
|
||||
if(page==13)
|
||||
p=addr13;
|
||||
if(page==14)
|
||||
p=addr14;
|
||||
if(page==15)
|
||||
p=addr15;
|
||||
if(page==16)
|
||||
p=addr16;
|
||||
if(page==17)
|
||||
p=addr17;
|
||||
if(page==18)
|
||||
p=addr18;
|
||||
if(page==19)
|
||||
p=addr19;
|
||||
if(page==20)
|
||||
p=addr20;
|
||||
if(page==21)
|
||||
p=addr21;
|
||||
if(page==22)
|
||||
p=addr22;
|
||||
if(page==23)
|
||||
p=addr23;
|
||||
if(page==24)
|
||||
p=addr24;
|
||||
if(page==25)
|
||||
p=addr25;
|
||||
if(page==26)
|
||||
p=addr26;
|
||||
if(page==27)
|
||||
p=addr27;
|
||||
if(page==28)
|
||||
p=addr28;
|
||||
if(page==29)
|
||||
p=addr29;
|
||||
if(page==30)
|
||||
p=addr30;
|
||||
if(page==31)
|
||||
p=addr31;
|
||||
if(page==32)
|
||||
p=addr32;
|
||||
if(page==33)
|
||||
p=addr33;
|
||||
if(page==34)
|
||||
p=addr34;
|
||||
if(page==35)
|
||||
p=addr35;
|
||||
if(page==36)
|
||||
p=addr36;
|
||||
if(page==37)
|
||||
p=addr37;
|
||||
if(page==38)
|
||||
p=addr38;
|
||||
if(page==39)
|
||||
p=addr39;
|
||||
if(page==40)
|
||||
p=addr40;
|
||||
if(page==41)
|
||||
p=addr41;
|
||||
if(page==42)
|
||||
p=addr42;
|
||||
if(page==43)
|
||||
p=addr43;
|
||||
if(page==44)
|
||||
p=addr44;
|
||||
if(page==45)
|
||||
p=addr45;
|
||||
if(page==46)
|
||||
p=addr46;
|
||||
if(page==47)
|
||||
p=addr47;
|
||||
if(page==48)
|
||||
p=addr48;
|
||||
if(page==49)
|
||||
p=addr49;
|
||||
if(page==50)
|
||||
p=addr50;
|
||||
if(page==51)
|
||||
p=addr51;
|
||||
if(page==52)
|
||||
p=addr52;
|
||||
if(page==53)
|
||||
p=addr53;
|
||||
if(page==54)
|
||||
p=addr54;
|
||||
if(page==55)
|
||||
p=addr55;
|
||||
if(page==56)
|
||||
p=addr56;
|
||||
if(page==57)
|
||||
p=addr57;
|
||||
|
||||
//p=addr00;
|
||||
uint32_t start=(page-1)*0x78;
|
||||
|
||||
DEBUG_MSG("\rpage %d ", page);
|
||||
if (PIOS_I2C_LockDevice(5 / portTICK_RATE_MS))
|
||||
{
|
||||
uint8_t cmd[5];
|
||||
cmd[0] = 0x01;
|
||||
cmd[1] = 0x7D;
|
||||
cmd[2] = (uint8_t)(start & 0xFF);
|
||||
cmd[3] = (uint8_t)(start >> 8);
|
||||
cmd[4] = 0x78;
|
||||
|
||||
/*PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 0);
|
||||
int32_t error = PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 2);
|
||||
DEBUG_MSG("\rerrx1 %d ", error);
|
||||
// Now receive byte(s)
|
||||
if(!error) {
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 0);
|
||||
//error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, addr78, sizeof(addr78));
|
||||
error = PIOS_I2C_Transfer(I2C_Read, 0x30<<1, buffer, 1);
|
||||
}
|
||||
DEBUG_MSG("\rerrx2 %d ", error);
|
||||
DEBUG_MSG("\ransx00 0x%02x ", buffer[0]);
|
||||
*/
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, buffer, 0);
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 5);
|
||||
/*if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, addr00, sizeof(addr00)-1) == 0)
|
||||
{
|
||||
if (PIOS_I2C_Transfer(I2C_Read, 0x30<<1, buffer, 1) == 0)
|
||||
{
|
||||
DEBUG_MSG("\rans00 0x%02x ", buffer[0]);
|
||||
}
|
||||
}*/
|
||||
PIOS_I2C_Transfer(I2C_Write, 0x30<<1, cmd, 0);
|
||||
//error = PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 5);
|
||||
//error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, &p[5], 120);
|
||||
//error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, p, p[4]+5);
|
||||
error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, p, 125);
|
||||
|
||||
DEBUG_MSG("\rerr1 %d ", error);
|
||||
/* Now receive byte(s) */
|
||||
if(!error) {
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 0);
|
||||
//error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, addr78, sizeof(addr78));
|
||||
//error = PIOS_I2C_Transfer(I2C_Read, 0x30<<1, buffer, 10);
|
||||
error = PIOS_I2C_Transfer(I2C_Write, 0x30<<1, cmd, 0);
|
||||
}
|
||||
DEBUG_MSG("\rerr2 %d ", error);
|
||||
DEBUG_MSG("\rans00 0x%02x ", buffer[0]);
|
||||
/*PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, cmd, 0);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x30<<1, addr78, 10) == 0)
|
||||
{
|
||||
if (PIOS_I2C_Transfer(I2C_Read, 0x30<<1, buffer, 1) == 0)
|
||||
{
|
||||
DEBUG_MSG("\rans78 0x%02x ", buffer[0]);
|
||||
}
|
||||
}*/
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
if(error == 0)
|
||||
retry_count=0;
|
||||
else
|
||||
{
|
||||
retry_count++;
|
||||
DEBUG_MSG("\rretry %d", retry_count);
|
||||
if(retry_count==5) //retry 5 times
|
||||
{
|
||||
retry_count=0;
|
||||
error=0;
|
||||
}
|
||||
}
|
||||
return error;
|
||||
}
|
||||
|
||||
static void VerifyConfig(void)
|
||||
{
|
||||
uint8_t buf[0x78];
|
||||
uint32_t addr=0;
|
||||
uint32_t n=0x78;
|
||||
uint8_t * p;
|
||||
uint8_t page;
|
||||
bool ok;
|
||||
while (addr<CONFIG_LENGTH)
|
||||
{
|
||||
n = MIN(CONFIG_LENGTH-addr, sizeof(buf));
|
||||
ok = FALSE;
|
||||
while (!ok)
|
||||
{
|
||||
if (Read(addr, n, buf))
|
||||
{
|
||||
page=(addr/0x78)+1;
|
||||
|
||||
if(page==1)
|
||||
p=addr01;
|
||||
if(page==2)
|
||||
p=addr02;
|
||||
if(page==3)
|
||||
p=addr03;
|
||||
if(page==4)
|
||||
p=addr04;
|
||||
if(page==5)
|
||||
p=addr05;
|
||||
if(page==6)
|
||||
p=addr06;
|
||||
if(page==7)
|
||||
p=addr07;
|
||||
if(page==8)
|
||||
p=addr08;
|
||||
if(page==9)
|
||||
p=addr09;
|
||||
if(page==10)
|
||||
p=addr10;
|
||||
if(page==11)
|
||||
p=addr11;
|
||||
if(page==12)
|
||||
p=addr12;
|
||||
if(page==13)
|
||||
p=addr13;
|
||||
if(page==14)
|
||||
p=addr14;
|
||||
if(page==15)
|
||||
p=addr15;
|
||||
if(page==16)
|
||||
p=addr16;
|
||||
if(page==17)
|
||||
p=addr17;
|
||||
if(page==18)
|
||||
p=addr18;
|
||||
if(page==19)
|
||||
p=addr19;
|
||||
if(page==20)
|
||||
p=addr20;
|
||||
if(page==21)
|
||||
p=addr21;
|
||||
if(page==22)
|
||||
p=addr22;
|
||||
if(page==23)
|
||||
p=addr23;
|
||||
if(page==24)
|
||||
p=addr24;
|
||||
if(page==25)
|
||||
p=addr25;
|
||||
if(page==26)
|
||||
p=addr26;
|
||||
if(page==27)
|
||||
p=addr27;
|
||||
if(page==28)
|
||||
p=addr28;
|
||||
if(page==29)
|
||||
p=addr29;
|
||||
if(page==30)
|
||||
p=addr30;
|
||||
if(page==31)
|
||||
p=addr31;
|
||||
if(page==32)
|
||||
p=addr32;
|
||||
if(page==33)
|
||||
p=addr33;
|
||||
if(page==34)
|
||||
p=addr34;
|
||||
if(page==35)
|
||||
p=addr35;
|
||||
if(page==36)
|
||||
p=addr36;
|
||||
if(page==37)
|
||||
p=addr37;
|
||||
if(page==38)
|
||||
p=addr38;
|
||||
if(page==39)
|
||||
p=addr39;
|
||||
if(page==40)
|
||||
p=addr40;
|
||||
if(page==41)
|
||||
p=addr41;
|
||||
if(page==42)
|
||||
p=addr42;
|
||||
if(page==43)
|
||||
p=addr43;
|
||||
if(page==44)
|
||||
p=addr44;
|
||||
if(page==45)
|
||||
p=addr45;
|
||||
if(page==46)
|
||||
p=addr46;
|
||||
if(page==47)
|
||||
p=addr47;
|
||||
if(page==48)
|
||||
p=addr48;
|
||||
if(page==49)
|
||||
p=addr49;
|
||||
if(page==50)
|
||||
p=addr50;
|
||||
if(page==51)
|
||||
p=addr51;
|
||||
if(page==52)
|
||||
p=addr52;
|
||||
if(page==53)
|
||||
p=addr53;
|
||||
if(page==54)
|
||||
p=addr54;
|
||||
if(page==55)
|
||||
p=addr55;
|
||||
if(page==56)
|
||||
p=addr56;
|
||||
if(page==57)
|
||||
p=addr57;
|
||||
|
||||
if(!memcmp(buf,&p[5],n))
|
||||
{
|
||||
DEBUG_MSG("\raddr %04x: OK",addr);
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("\raddr %04x:",addr);
|
||||
for(uint32_t i=0; i<n; i++)
|
||||
DEBUG_MSG("0x%02x, ", buf[i]);
|
||||
}
|
||||
ok = TRUE;
|
||||
}
|
||||
}
|
||||
DEBUG_MSG("\n\r");
|
||||
addr += n;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#ifdef DO_PAR_SEEK
|
||||
|
||||
static void DoParSeek(void)
|
||||
{
|
||||
int pos=3;
|
||||
char save;
|
||||
while(1)
|
||||
{
|
||||
int i;
|
||||
|
||||
SetVoltage(pos*1000+100);
|
||||
save = msg[pos];
|
||||
for (i=0;i<256; i+=5)
|
||||
{
|
||||
//msg[pos]=i;
|
||||
DEBUG_MSG("SendMsg .");
|
||||
if (PIOS_I2C_LockDevice(5000 / portTICK_RATE_MS))
|
||||
{
|
||||
DEBUG_MSG(".");
|
||||
PIOS_I2C_Transfer(I2C_Write, 0x30<<1, msg, sizeof(msg));
|
||||
DEBUG_MSG(".");
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
DEBUG_MSG("\n\r");
|
||||
|
||||
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
}
|
||||
msg[pos] = save;
|
||||
pos+=1;
|
||||
|
||||
if (pos == OSDMSG_V_LS_IDX || pos == OSDMSG_VA_MS_IDX)
|
||||
pos++;
|
||||
|
||||
if (pos>sizeof(msg))
|
||||
pos = 3;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
static void Task(void* parameters)
|
||||
{
|
||||
uint32_t cnt = 0;
|
||||
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
||||
#endif
|
||||
|
||||
PositionActualConnectCallback(PositionActualUpdatedCb);
|
||||
FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb);
|
||||
|
||||
#ifdef WRITE_CONFIG
|
||||
vTaskDelay(1000 / portTICK_RATE_MS);
|
||||
DEBUG_MSG("Write Start\n");
|
||||
for(int i=1;i<58;i++)
|
||||
{
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
if(WriteConfig(i))
|
||||
i--;
|
||||
}
|
||||
VerifyConfig();
|
||||
DEBUG_MSG("Write End\n");
|
||||
while(1)
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef DUMP_CONFIG
|
||||
vTaskDelay(1000 / portTICK_RATE_MS);
|
||||
DEBUG_MSG("DUMP Start\n");
|
||||
DumpConfig();
|
||||
DEBUG_MSG("DUMP End\n");
|
||||
while(1)
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
#endif
|
||||
|
||||
DEBUG_MSG("OSD ET Std Started\n");
|
||||
vTaskDelay(2000 / portTICK_RATE_MS);
|
||||
|
||||
#ifdef DO_PAR_SEEK
|
||||
DoParSeek();
|
||||
#endif
|
||||
|
||||
while (1)
|
||||
{
|
||||
DEBUG_MSG("%d\n\r", cnt);
|
||||
#if 1
|
||||
if ( newBattData )
|
||||
{
|
||||
FlightBatteryStateData flightBatteryData;
|
||||
|
||||
FlightBatteryStateGet(&flightBatteryData);
|
||||
|
||||
DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000));
|
||||
|
||||
SetVoltage((uint32_t)(flightBatteryData.Voltage*1000));
|
||||
SetCurrent((uint32_t)(flightBatteryData.Current*1000));
|
||||
newBattData = FALSE;
|
||||
}
|
||||
|
||||
|
||||
if (newPosData)
|
||||
{
|
||||
PositionActualData positionData;
|
||||
|
||||
PositionActualGet(&positionData);
|
||||
|
||||
DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt,
|
||||
positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);
|
||||
|
||||
// GPS Status
|
||||
if (positionData.Status == POSITIONACTUAL_STATUS_FIX3D)
|
||||
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
|
||||
else
|
||||
msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;
|
||||
msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG;
|
||||
|
||||
|
||||
// GPS info
|
||||
SetCoord(OSDMSG_LAT_IDX, positionData.Latitude);
|
||||
SetCoord(OSDMSG_LON_IDX, positionData.Longitude);
|
||||
SetAltitude(positionData.Altitude);
|
||||
SetNbSats(positionData.Satellites);
|
||||
SetCourse(positionData.Heading);
|
||||
|
||||
newPosData = FALSE;
|
||||
}
|
||||
else
|
||||
{
|
||||
msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG;
|
||||
}
|
||||
#endif
|
||||
|
||||
DEBUG_MSG("SendMsg .");
|
||||
if (PIOS_I2C_LockDevice(5000 / portTICK_RATE_MS))
|
||||
{
|
||||
DEBUG_MSG(".");
|
||||
PIOS_I2C_Transfer(I2C_Write, 0x30<<1, msg, sizeof(msg));
|
||||
DEBUG_MSG(".");
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
DEBUG_MSG("\n\r");
|
||||
|
||||
cnt++;
|
||||
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// Public functions
|
||||
//
|
||||
|
||||
/**
|
||||
* Initialise the module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t OsdEtStdInitialize(void)
|
||||
{
|
||||
// Start gps task
|
||||
xTaskCreate(Task, (signed char*) "Osd", STACK_SIZE, NULL, TASK_PRIORITY, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
@ -1,31 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file stabilization.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef STABILIZATION_H
|
||||
#define STABILIZATION_H
|
||||
|
||||
int32_t StabilizationInitialize();
|
||||
|
||||
#endif // STABILIZATION_H
|
@ -1,180 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file stabilization.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "stabilization.h"
|
||||
#include "stabilizationsettings.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "attitudedesired.h"
|
||||
#include "attitudeactual.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
#include "systemsettings.h"
|
||||
|
||||
// Private constants
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
|
||||
#define PITCH_INTEGRAL_LIMIT 0.5
|
||||
#define ROLL_INTEGRAL_LIMIT 0.5
|
||||
#define YAW_INTEGRAL_LIMIT 0.5
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// Private functions
|
||||
static void stabilizationTask(void* parameters);
|
||||
static float bound(float val, float min, float max);
|
||||
|
||||
/**
|
||||
* Module initialization
|
||||
*/
|
||||
int32_t StabilizationInitialize()
|
||||
{
|
||||
// Initialize variables
|
||||
|
||||
// Start main task
|
||||
xTaskCreate(stabilizationTask, (signed char*)"Stabilization", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Module task
|
||||
*/
|
||||
static void stabilizationTask(void* parameters)
|
||||
{
|
||||
StabilizationSettingsData stabSettings;
|
||||
ActuatorDesiredData actuatorDesired;
|
||||
AttitudeDesiredData attitudeDesired;
|
||||
AttitudeActualData attitudeActual;
|
||||
ManualControlCommandData manualControl;
|
||||
SystemSettingsData systemSettings;
|
||||
portTickType lastSysTime;
|
||||
float pitchError, pitchErrorLast;
|
||||
float rollError, rollErrorLast;
|
||||
float yawError, yawErrorLast;
|
||||
float pitchDerivative;
|
||||
float rollDerivative;
|
||||
float yawDerivative;
|
||||
float pitchIntegral, pitchIntegralLimit;
|
||||
float rollIntegral, rollIntegralLimit;
|
||||
float yawIntegral, yawIntegralLimit;
|
||||
|
||||
// Initialize
|
||||
pitchIntegral = 0.0;
|
||||
rollIntegral = 0.0;
|
||||
yawIntegral = 0.0;
|
||||
pitchErrorLast = 0.0;
|
||||
rollErrorLast = 0.0;
|
||||
yawErrorLast = 0.0;
|
||||
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1)
|
||||
{
|
||||
// Read settings and other objects
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
ManualControlCommandGet(&manualControl);
|
||||
AttitudeDesiredGet(&attitudeDesired);
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
|
||||
// Pitch stabilization control loop
|
||||
pitchError = bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch;
|
||||
pitchDerivative = pitchError - pitchErrorLast;
|
||||
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
|
||||
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
|
||||
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
|
||||
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
|
||||
pitchErrorLast = pitchError;
|
||||
|
||||
// Roll stabilization control loop
|
||||
rollError = bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll;
|
||||
rollDerivative = rollError - rollErrorLast;
|
||||
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
|
||||
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
|
||||
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
|
||||
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
|
||||
rollErrorLast = rollError;
|
||||
|
||||
// Yaw stabilization control loop (only enabled on VTOL airframes)
|
||||
if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )
|
||||
{
|
||||
yawError = attitudeDesired.Yaw - attitudeActual.Yaw;
|
||||
yawDerivative = yawError - yawErrorLast;
|
||||
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
|
||||
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
|
||||
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
|
||||
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
|
||||
yawErrorLast = yawError;
|
||||
}
|
||||
else
|
||||
{
|
||||
actuatorDesired.Yaw = 0.0;
|
||||
}
|
||||
|
||||
// Setup throttle
|
||||
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
|
||||
|
||||
// Write actuator desired (if not in manual mode)
|
||||
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
|
||||
{
|
||||
ActuatorDesiredSet(&actuatorDesired);
|
||||
}
|
||||
else
|
||||
{
|
||||
pitchIntegral = 0.0;
|
||||
rollIntegral = 0.0;
|
||||
yawIntegral = 0.0;
|
||||
pitchErrorLast = 0.0;
|
||||
rollErrorLast = 0.0;
|
||||
yawErrorLast = 0.0;
|
||||
}
|
||||
|
||||
// Clear alarms
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
|
||||
|
||||
// Wait until next update
|
||||
vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Bound input value between limits
|
||||
*/
|
||||
static float bound(float val, float min, float max)
|
||||
{
|
||||
if ( val < min )
|
||||
{
|
||||
val = min;
|
||||
}
|
||||
else if ( val > max )
|
||||
{
|
||||
val = max;
|
||||
}
|
||||
return val;
|
||||
}
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemmod.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief System module
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef SYSTEMMOD_H
|
||||
#define SYSTEMMOD_H
|
||||
|
||||
int32_t SystemModInitialize(void);
|
||||
|
||||
#endif // SYSTEMMOD_H
|
||||
|
@ -1,319 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemmod.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief System module
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "systemmod.h"
|
||||
#include "objectpersistence.h"
|
||||
#include "systemstats.h"
|
||||
|
||||
// Private constants
|
||||
#define SYSTEM_UPDATE_PERIOD_MS 1000
|
||||
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
|
||||
// must be updated if the FreeRTOS or compiler
|
||||
// optimisation options are changed.
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+3)
|
||||
|
||||
#define HEAP_LIMIT_WARNING 4000
|
||||
#define HEAP_LIMIT_CRITICAL 1000
|
||||
#define CPULOAD_LIMIT_WARNING 80
|
||||
#define CPULOAD_LIMIT_CRITICAL 95
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static uint32_t idleCounter;
|
||||
static uint32_t idleCounterClear;
|
||||
static xTaskHandle systemTaskHandle;
|
||||
static int32_t stackOverflow;
|
||||
|
||||
// Private functions
|
||||
static void objectUpdatedCb(UAVObjEvent* ev);
|
||||
static void updateStats();
|
||||
static void updateSystemAlarms();
|
||||
static void systemTask(void* parameters);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup.
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t SystemModInitialize(void)
|
||||
{
|
||||
// Initialize vars
|
||||
stackOverflow = 0;
|
||||
// Create system task
|
||||
xTaskCreate(systemTask, (signed char*)"System", STACK_SIZE, NULL, TASK_PRIORITY, &systemTaskHandle);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
|
||||
*/
|
||||
static void systemTask(void* parameters)
|
||||
{
|
||||
portTickType lastSysTime;
|
||||
|
||||
// System initialization
|
||||
OpenPilotInit();
|
||||
|
||||
// Initialize vars
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
|
||||
// Listen for SettingPersistance object updates, connect a callback function
|
||||
ObjectPersistenceConnectCallback(&objectUpdatedCb);
|
||||
|
||||
// Main system loop
|
||||
while (1)
|
||||
{
|
||||
// Update the system statistics
|
||||
updateStats();
|
||||
|
||||
// Update the system alarms
|
||||
updateSystemAlarms();
|
||||
|
||||
// Flash the heartbeat LED
|
||||
PIOS_LED_Toggle(LED1);
|
||||
|
||||
// Turn on the error LED if an alarm is set
|
||||
if ( AlarmsHasWarnings() )
|
||||
{
|
||||
PIOS_LED_On(LED2);
|
||||
}
|
||||
else
|
||||
{
|
||||
PIOS_LED_Off(LED2);
|
||||
}
|
||||
|
||||
// Wait until next period
|
||||
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Function called in response to object updates
|
||||
*/
|
||||
static void objectUpdatedCb(UAVObjEvent* ev)
|
||||
{
|
||||
ObjectPersistenceData objper;
|
||||
UAVObjHandle obj;
|
||||
|
||||
// If the object updated was the ObjectPersistence execute requested action
|
||||
if ( ev->obj == ObjectPersistenceHandle() )
|
||||
{
|
||||
// Get object data
|
||||
ObjectPersistenceGet(&objper);
|
||||
|
||||
// Execute action
|
||||
if ( objper.Operation == OBJECTPERSISTENCE_OPERATION_LOAD)
|
||||
{
|
||||
if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT )
|
||||
{
|
||||
// Get selected object
|
||||
obj = UAVObjGetByID(objper.ObjectID);
|
||||
if ( obj == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
// Load selected instance
|
||||
UAVObjLoad(obj, objper.InstanceID);
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjLoadSettings();
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjLoadMetaobjects();
|
||||
}
|
||||
}
|
||||
else if ( objper.Operation == OBJECTPERSISTENCE_OPERATION_SAVE)
|
||||
{
|
||||
if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT )
|
||||
{
|
||||
// Get selected object
|
||||
obj = UAVObjGetByID(objper.ObjectID);
|
||||
if ( obj == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
// Save selected instance
|
||||
UAVObjSave(obj, objper.InstanceID);
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjSaveSettings();
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjSaveMetaobjects();
|
||||
}
|
||||
}
|
||||
else if ( objper.Operation == OBJECTPERSISTENCE_OPERATION_DELETE)
|
||||
{
|
||||
if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT )
|
||||
{
|
||||
// Get selected object
|
||||
obj = UAVObjGetByID(objper.ObjectID);
|
||||
if ( obj == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
// Delete selected instance
|
||||
UAVObjDelete(obj, objper.InstanceID);
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLSETTINGS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjDeleteSettings();
|
||||
}
|
||||
else if ( objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS)
|
||||
{
|
||||
UAVObjDeleteMetaobjects();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called periodically to update the system stats
|
||||
*/
|
||||
static void updateStats()
|
||||
{
|
||||
SystemStatsData stats;
|
||||
|
||||
// Get stats and update
|
||||
SystemStatsGet(&stats);
|
||||
stats.FlightTime = xTaskGetTickCount()*portTICK_RATE_MS;
|
||||
#ifdef ARCH_POSIX
|
||||
// POSIX port of FreeRTOS doesn't have xPortGetFreeHeapSize()
|
||||
stats.HeapRemaining = 10240;
|
||||
#else
|
||||
stats.HeapRemaining = xPortGetFreeHeapSize();
|
||||
#endif
|
||||
stats.CPULoad = 100 - (uint8_t)round(100.0*((float)idleCounter/(float)(SYSTEM_UPDATE_PERIOD_MS/1000))/(float)IDLE_COUNTS_PER_SEC_AT_NO_LOAD);
|
||||
idleCounterClear = 1;
|
||||
SystemStatsSet(&stats);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update system alarms
|
||||
*/
|
||||
static void updateSystemAlarms()
|
||||
{
|
||||
SystemStatsData stats;
|
||||
UAVObjStats objStats;
|
||||
EventStats evStats;
|
||||
SystemStatsGet(&stats);
|
||||
|
||||
// Check heap
|
||||
if ( stats.HeapRemaining < HEAP_LIMIT_CRITICAL )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else if ( stats.HeapRemaining < HEAP_LIMIT_WARNING )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY);
|
||||
}
|
||||
|
||||
// Check CPU load
|
||||
if ( stats.CPULoad > CPULOAD_LIMIT_CRITICAL )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else if ( stats.CPULoad > CPULOAD_LIMIT_WARNING )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD);
|
||||
}
|
||||
|
||||
// Check for stack overflow
|
||||
if ( stackOverflow == 1 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
|
||||
}
|
||||
|
||||
// Check for SD card
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_SDCARD, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_SDCARD);
|
||||
}
|
||||
|
||||
// Check for event errors
|
||||
UAVObjGetStats(&objStats);
|
||||
EventGetStats(&evStats);
|
||||
UAVObjClearStats();
|
||||
EventClearStats();
|
||||
if ( objStats.eventErrors > 0 || evStats.eventErrors > 0 )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
// Called when the scheduler has no tasks to run
|
||||
if (idleCounterClear == 0)
|
||||
{
|
||||
++idleCounter;
|
||||
}
|
||||
else
|
||||
{
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Called by the RTOS when a stack overflow is detected.
|
||||
*/
|
||||
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
|
||||
{
|
||||
stackOverflow = 1;
|
||||
}
|
||||
|
@ -1,34 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file telemetry.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the telemetry module.
|
||||
* As with all modules only the initialize function is exposed all other
|
||||
* interactions with the module take place through the event queue and
|
||||
* objects.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef TELEMETRY_H
|
||||
#define TELEMETRY_H
|
||||
|
||||
int32_t TelemetryInitialize(void);
|
||||
|
||||
#endif // TELEMETRY_H
|
@ -1,553 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file telemetry.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Telemetry module, handles telemetry and UAVObject updates
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "flighttelemetrystats.h"
|
||||
#include "gcstelemetrystats.h"
|
||||
#include "telemetrysettings.h"
|
||||
|
||||
// Set this to 1 to enable telemetry via the USB HID interface
|
||||
#define ALLOW_HID_TELEMETRY 0
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 20
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2)
|
||||
#define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2)
|
||||
#define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2)
|
||||
#define REQ_TIMEOUT_MS 250
|
||||
#define MAX_RETRIES 2
|
||||
#define STATS_UPDATE_PERIOD_MS 4000
|
||||
#define CONNECTION_TIMEOUT_MS 8000
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static uint8_t telemetryPort;
|
||||
static xQueueHandle queue;
|
||||
static xQueueHandle priorityQueue;
|
||||
static xTaskHandle telemetryTxTaskHandle;
|
||||
static xTaskHandle telemetryTxPriTaskHandle;
|
||||
static xTaskHandle telemetryRxTaskHandle;
|
||||
static uint32_t txErrors;
|
||||
static uint32_t txRetries;
|
||||
static TelemetrySettingsData settings;
|
||||
static uint32_t timeOfLastObjectUpdate;
|
||||
|
||||
// Private functions
|
||||
static void telemetryTxTask(void* parameters);
|
||||
static void telemetryTxPriTask(void* parameters);
|
||||
static void telemetryRxTask(void* parameters);
|
||||
static int32_t transmitData(uint8_t* data, int32_t length);
|
||||
static void registerObject(UAVObjHandle obj);
|
||||
static void updateObject(UAVObjHandle obj);
|
||||
static int32_t addObject(UAVObjHandle obj);
|
||||
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
|
||||
static void processObjEvent(UAVObjEvent* ev);
|
||||
static void updateTelemetryStats();
|
||||
static void gcsTelemetryStatsUpdated();
|
||||
static void updateSettings();
|
||||
|
||||
/**
|
||||
* Initialise the telemetry module
|
||||
* \return -1 if initialisation failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t TelemetryInitialize(void)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Initialize vars
|
||||
timeOfLastObjectUpdate = 0;
|
||||
|
||||
// Create object queues
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
||||
|
||||
// Get telemetry settings object
|
||||
updateSettings();
|
||||
|
||||
// Initialise UAVTalk
|
||||
UAVTalkInitialize(&transmitData);
|
||||
|
||||
// Process all registered objects and connect queue for updates
|
||||
UAVObjIterate(®isterObject);
|
||||
|
||||
// Create periodic event that will be used to update the telemetry stats
|
||||
txErrors = 0;
|
||||
txRetries = 0;
|
||||
memset(&ev, 0, sizeof(UAVObjEvent));
|
||||
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
|
||||
|
||||
// Listen to objects of interest
|
||||
GCSTelemetryStatsConnectQueue(priorityQueue);
|
||||
TelemetrySettingsConnectQueue(priorityQueue);
|
||||
|
||||
// Start telemetry tasks
|
||||
xTaskCreate(telemetryTxTask, (signed char*)"TelTx", STACK_SIZE, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
|
||||
xTaskCreate(telemetryTxPriTask, (signed char*)"TelPriTx", STACK_SIZE, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle);
|
||||
xTaskCreate(telemetryRxTask, (signed char*)"TelRx", STACK_SIZE, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Register a new object, adds object to local list and connects the queue depending on the object's
|
||||
* telemetry settings.
|
||||
* \param[in] obj Object to connect
|
||||
*/
|
||||
static void registerObject(UAVObjHandle obj)
|
||||
{
|
||||
// Setup object for periodic updates
|
||||
addObject(obj);
|
||||
|
||||
// Setup object for telemetry updates
|
||||
updateObject(obj);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update object's queue connections and timer, depending on object's settings
|
||||
* \param[in] obj Object to updates
|
||||
*/
|
||||
static void updateObject(UAVObjHandle obj)
|
||||
{
|
||||
UAVObjMetadata metadata;
|
||||
int32_t eventMask;
|
||||
|
||||
// Get metadata
|
||||
UAVObjGetMetadata(obj, &metadata);
|
||||
|
||||
// Setup object depending on update mode
|
||||
if(metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
|
||||
{
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
|
||||
// Connect queue
|
||||
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
if(UAVObjIsMetaobject(obj))
|
||||
{
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
}
|
||||
else if(metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
|
||||
{
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Connect queue
|
||||
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
if(UAVObjIsMetaobject(obj))
|
||||
{
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
}
|
||||
else if(metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
|
||||
{
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Connect queue
|
||||
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
|
||||
if(UAVObjIsMetaobject(obj))
|
||||
{
|
||||
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
|
||||
}
|
||||
UAVObjConnectQueue(obj, priorityQueue, eventMask);
|
||||
}
|
||||
else if(metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
|
||||
{
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Disconnect queue
|
||||
UAVObjDisconnectQueue(obj, priorityQueue);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Processes queue events
|
||||
*/
|
||||
static void processObjEvent(UAVObjEvent* ev)
|
||||
{
|
||||
UAVObjMetadata metadata;
|
||||
FlightTelemetryStatsData flightStats;
|
||||
int32_t retries;
|
||||
int32_t success;
|
||||
|
||||
if ( ev->obj == 0 )
|
||||
{
|
||||
updateTelemetryStats();
|
||||
}
|
||||
else if ( ev->obj == GCSTelemetryStatsHandle() )
|
||||
{
|
||||
gcsTelemetryStatsUpdated();
|
||||
}
|
||||
else if ( ev->obj == TelemetrySettingsHandle() )
|
||||
{
|
||||
updateSettings();
|
||||
}
|
||||
else
|
||||
{
|
||||
// Only process event if connected to GCS or if object FlightTelemetryStats is updated
|
||||
FlightTelemetryStatsGet(&flightStats);
|
||||
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle() )
|
||||
{
|
||||
// Get object metadata
|
||||
UAVObjGetMetadata(ev->obj, &metadata);
|
||||
// Act on event
|
||||
retries = 0;
|
||||
success = -1;
|
||||
if(ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL)
|
||||
{
|
||||
// Send update to GCS (with retries)
|
||||
while(retries < MAX_RETRIES && success == -1)
|
||||
{
|
||||
success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
++retries;
|
||||
}
|
||||
// Update stats
|
||||
txRetries += (retries-1);
|
||||
if ( success == -1 )
|
||||
{
|
||||
++txErrors;
|
||||
}
|
||||
}
|
||||
else if(ev->event == EV_UPDATE_REQ)
|
||||
{
|
||||
// Request object update from GCS (with retries)
|
||||
while(retries < MAX_RETRIES && success == -1)
|
||||
{
|
||||
success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
|
||||
++retries;
|
||||
}
|
||||
// Update stats
|
||||
txRetries += (retries-1);
|
||||
if ( success == -1 )
|
||||
{
|
||||
++txErrors;
|
||||
}
|
||||
}
|
||||
// If this is a metaobject then make necessary telemetry updates
|
||||
if(UAVObjIsMetaobject(ev->obj))
|
||||
{
|
||||
updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Telemetry transmit task, regular priority
|
||||
*/
|
||||
static void telemetryTxTask(void* parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Loop forever
|
||||
while(1)
|
||||
{
|
||||
// Wait for queue message
|
||||
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
// Process event
|
||||
processObjEvent(&ev);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Telemetry transmit task, high priority
|
||||
*/
|
||||
static void telemetryTxPriTask(void* parameters)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Loop forever
|
||||
while(1)
|
||||
{
|
||||
// Wait for queue message
|
||||
if(xQueueReceive(priorityQueue, &ev, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
// Process event
|
||||
processObjEvent(&ev);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Telemetry transmit task. Processes queue events and periodic updates.
|
||||
*/
|
||||
static void telemetryRxTask(void* parameters)
|
||||
{
|
||||
uint8_t inputPort;
|
||||
int32_t len;
|
||||
|
||||
// Task loop
|
||||
while (1)
|
||||
{
|
||||
#if ALLOW_HID_TELEMETRY
|
||||
// Determine input port (USB takes priority over telemetry port)
|
||||
if(PIOS_USB_HID_CheckAvailable(0))
|
||||
{
|
||||
inputPort = PIOS_COM_TELEM_USB;
|
||||
}
|
||||
else
|
||||
#endif /* ALLOW_HID_TELEMETRY */
|
||||
{
|
||||
inputPort = telemetryPort;
|
||||
}
|
||||
|
||||
// Block until data are available
|
||||
// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
|
||||
len = PIOS_COM_ReceiveBufferUsed(inputPort);
|
||||
for (int32_t n = 0; n < len; ++n)
|
||||
{
|
||||
//PIOS_LED_Toggle(LED1);
|
||||
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort));
|
||||
}
|
||||
vTaskDelay(5); // <- remove when blocking calls are implemented
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Transmit data buffer to the modem or USB port.
|
||||
* \param[in] data Data buffer to send
|
||||
* \param[in] length Length of buffer
|
||||
* \return 0 Success
|
||||
*/
|
||||
static int32_t transmitData(uint8_t* data, int32_t length)
|
||||
{
|
||||
uint8_t outputPort;
|
||||
|
||||
// Determine input port (USB takes priority over telemetry port)
|
||||
#if ALLOW_HID_TELEMETRY
|
||||
if(PIOS_USB_HID_CheckAvailable(0))
|
||||
{
|
||||
outputPort = PIOS_COM_TELEM_USB;
|
||||
}
|
||||
else
|
||||
#endif /* ALLOW_HID_TELEMETRY */
|
||||
{
|
||||
outputPort = telemetryPort;
|
||||
}
|
||||
|
||||
// TODO: Update once the PIOS_COM is made blocking (it is implemented as a busy loop for now!)
|
||||
//PIOS_LED_Toggle(LED2);
|
||||
return PIOS_COM_SendBuffer(outputPort, data, length);
|
||||
}
|
||||
|
||||
/**
|
||||
* Setup object for periodic updates.
|
||||
* \param[in] obj The object to update
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t addObject(UAVObjHandle obj)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Add object for periodic updates
|
||||
ev.obj = obj;
|
||||
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||
ev.event = EV_UPDATED_MANUAL;
|
||||
return EventPeriodicQueueCreate(&ev, queue, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set update period of object (it must be already setup for periodic updates)
|
||||
* \param[in] obj The object to update
|
||||
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
// Add object for periodic updates
|
||||
ev.obj = obj;
|
||||
ev.instId = UAVOBJ_ALL_INSTANCES;
|
||||
ev.event = EV_UPDATED_MANUAL;
|
||||
return EventPeriodicQueueUpdate(&ev, queue, updatePeriodMs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Called each time the GCS telemetry stats object is updated.
|
||||
* Trigger a flight telemetry stats update if a connection is not
|
||||
* yet established.
|
||||
*/
|
||||
static void gcsTelemetryStatsUpdated()
|
||||
{
|
||||
FlightTelemetryStatsData flightStats;
|
||||
GCSTelemetryStatsData gcsStats;
|
||||
FlightTelemetryStatsGet(&flightStats);
|
||||
GCSTelemetryStatsGet(&gcsStats);
|
||||
if ( flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED ||
|
||||
gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED )
|
||||
{
|
||||
updateTelemetryStats();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update telemetry statistics and handle connection handshake
|
||||
*/
|
||||
static void updateTelemetryStats()
|
||||
{
|
||||
UAVTalkStats utalkStats;
|
||||
FlightTelemetryStatsData flightStats;
|
||||
GCSTelemetryStatsData gcsStats;
|
||||
uint8_t forceUpdate;
|
||||
uint8_t connectionTimeout;
|
||||
uint32_t timeNow;
|
||||
|
||||
// Get stats
|
||||
UAVTalkGetStats(&utalkStats);
|
||||
UAVTalkResetStats();
|
||||
|
||||
// Get object data
|
||||
FlightTelemetryStatsGet(&flightStats);
|
||||
GCSTelemetryStatsGet(&gcsStats);
|
||||
|
||||
// Update stats object
|
||||
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
|
||||
{
|
||||
flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
|
||||
flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
|
||||
flightStats.RxFailures += utalkStats.rxErrors;
|
||||
flightStats.TxFailures += txErrors;
|
||||
flightStats.TxRetries += txRetries;
|
||||
txErrors = 0;
|
||||
txRetries = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
flightStats.RxDataRate = 0;
|
||||
flightStats.TxDataRate = 0;
|
||||
flightStats.RxFailures = 0;
|
||||
flightStats.TxFailures = 0;
|
||||
flightStats.TxRetries = 0;
|
||||
txErrors = 0;
|
||||
txRetries = 0;
|
||||
}
|
||||
|
||||
// Check for connection timeout
|
||||
timeNow = xTaskGetTickCount()*portTICK_RATE_MS;
|
||||
if ( utalkStats.rxObjects > 0 )
|
||||
{
|
||||
timeOfLastObjectUpdate = timeNow;
|
||||
}
|
||||
if ( (timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS )
|
||||
{
|
||||
connectionTimeout = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
connectionTimeout = 0;
|
||||
}
|
||||
|
||||
// Update connection state
|
||||
forceUpdate = 1;
|
||||
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED )
|
||||
{
|
||||
// Wait for connection request
|
||||
if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ )
|
||||
{
|
||||
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
|
||||
}
|
||||
}
|
||||
else if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK )
|
||||
{
|
||||
// Wait for connection
|
||||
if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED )
|
||||
{
|
||||
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
|
||||
}
|
||||
else if ( gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED )
|
||||
{
|
||||
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
|
||||
}
|
||||
}
|
||||
else if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
|
||||
{
|
||||
if ( gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout )
|
||||
{
|
||||
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
|
||||
}
|
||||
else
|
||||
{
|
||||
forceUpdate = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
|
||||
}
|
||||
|
||||
// Update the telemetry alarm
|
||||
if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED )
|
||||
{
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
|
||||
}
|
||||
else
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
|
||||
// Update object
|
||||
FlightTelemetryStatsSet(&flightStats);
|
||||
|
||||
// Force telemetry update if not connected
|
||||
if ( forceUpdate )
|
||||
{
|
||||
FlightTelemetryStatsUpdated();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the telemetry settings, called on startup and
|
||||
* each time the settings object is updated
|
||||
*/
|
||||
static void updateSettings()
|
||||
{
|
||||
// Set port
|
||||
telemetryPort = PIOS_COM_TELEM_RF;
|
||||
// Retrieve settings
|
||||
TelemetrySettingsGet(&settings);
|
||||
// Set port speed
|
||||
if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600)
|
||||
{
|
||||
PIOS_COM_ChangeBaud(telemetryPort, 9600);
|
||||
}
|
||||
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600)
|
||||
{
|
||||
PIOS_COM_ChangeBaud(telemetryPort, 57600);
|
||||
}
|
||||
else if (settings.Speed == TELEMETRYSETTINGS_SPEED_38400)
|
||||
{
|
||||
PIOS_COM_ChangeBaud(telemetryPort, 38400);
|
||||
}
|
||||
}
|
||||
|
@ -1,180 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file alarms.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Library for setting and clearing system alarms
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "alarms.h"
|
||||
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xSemaphoreHandle lock;
|
||||
|
||||
// Private functions
|
||||
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
|
||||
|
||||
/**
|
||||
* Initialize the alarms library
|
||||
*/
|
||||
int32_t AlarmsInitialize(void)
|
||||
{
|
||||
lock = xSemaphoreCreateRecursiveMutex();
|
||||
AlarmsClearAll();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set an alarm
|
||||
* @param alarm The system alarm to be modified
|
||||
* @param severity The alarm severity
|
||||
* @return 0 if success, -1 if an error
|
||||
*/
|
||||
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
|
||||
// Check that this is a valid alarm
|
||||
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
|
||||
// Read alarm and update its severity only if it was changed
|
||||
SystemAlarmsGet(&alarms);
|
||||
if ( alarms.Alarm[alarm] != severity )
|
||||
{
|
||||
alarms.Alarm[alarm] = severity;
|
||||
SystemAlarmsSet(&alarms);
|
||||
SystemAlarmsUpdated(); // force telemetry update since the alarm was changed
|
||||
}
|
||||
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get an alarm
|
||||
* @param alarm The system alarm to be read
|
||||
* @return Alarm severity
|
||||
*/
|
||||
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
|
||||
// Check that this is a valid alarm
|
||||
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Read alarm
|
||||
SystemAlarmsGet(&alarms);
|
||||
return alarms.Alarm[alarm];
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear an alarm
|
||||
* @param alarm The system alarm to be modified
|
||||
* @return 0 if success, -1 if an error
|
||||
*/
|
||||
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
|
||||
{
|
||||
return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear all alarms
|
||||
*/
|
||||
void AlarmsClearAll()
|
||||
{
|
||||
uint32_t n;
|
||||
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
|
||||
{
|
||||
AlarmsClear(n);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with the given or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasWarnings()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with error or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasErrors()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
|
||||
};
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with critical or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
int32_t AlarmsHasCritical()
|
||||
{
|
||||
return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
|
||||
};
|
||||
|
||||
/**
|
||||
* Check if there are any alarms with the given or higher severity
|
||||
* @return 0 if no alarms are found, 1 if at least one alarm is found
|
||||
*/
|
||||
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
|
||||
{
|
||||
SystemAlarmsData alarms;
|
||||
uint32_t n;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
|
||||
|
||||
// Read alarms
|
||||
SystemAlarmsGet(&alarms);
|
||||
|
||||
// Go through alarms and check if any are of the given severity or higher
|
||||
for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
|
||||
{
|
||||
if ( alarms.Alarm[n] >= severity)
|
||||
{
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// If this point is reached then no alarms found
|
||||
xSemaphoreGiveRecursive(lock);
|
||||
return 0;
|
||||
}
|
@ -1,40 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file alarms.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include file of the alarm library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef ALARMS_H
|
||||
#define ALARMS_H
|
||||
|
||||
#include "systemalarms.h"
|
||||
|
||||
int32_t AlarmsInitialize(void);
|
||||
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity);
|
||||
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm);
|
||||
int32_t AlarmsClear(SystemAlarmsAlarmElem alarm);
|
||||
void AlarmsClearAll();
|
||||
int32_t AlarmsHasWarnings();
|
||||
int32_t AlarmsHasErrors();
|
||||
int32_t AlarmsHasCritical();
|
||||
|
||||
#endif // ALARMS_H
|
||||
|
@ -1,31 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file op_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief OpenPilot configuration header.
|
||||
* Compile time config for OpenPilot Application
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef OP_CONFIG_H
|
||||
#define OP_CONFIG_H
|
||||
|
||||
#endif /* OP_CONFIG_H */
|
@ -1,45 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Main OpenPilot header.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef OPENPILOT_H
|
||||
#define OPENPILOT_H
|
||||
|
||||
|
||||
/* PIOS Includes */
|
||||
#include <pios.h>
|
||||
|
||||
/* OpenPilot Libraries */
|
||||
#include "op_config.h"
|
||||
#include "utlist.h"
|
||||
#include "uavobjectmanager.h"
|
||||
#include "eventdispatcher.h"
|
||||
#include "alarms.h"
|
||||
#include "uavtalk.h"
|
||||
|
||||
/* Global Functions */
|
||||
void OpenPilotInit(void);
|
||||
|
||||
#endif /* OPENPILOT_H */
|
@ -1,369 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board hardware for the OpenPilot Version 1.1 hardware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_BOARD_H
|
||||
#define PIOS_BOARD_H
|
||||
|
||||
|
||||
|
||||
//------------------------
|
||||
// Timers and Channels Used
|
||||
//------------------------
|
||||
/*
|
||||
Timer | Channel 1 | Channel 2 | Channel 3 | Channel 4
|
||||
------+-----------+-----------+-----------+----------
|
||||
TIM1 | RC In 3 | RC In 6 | RC In 5 |
|
||||
TIM2 | --------------- PIOS_DELAY -----------------
|
||||
TIM3 | RC In 7 | RC In 8 | RC In 1 | RC In 2
|
||||
TIM4 | Servo 1 | Servo 2 | Servo 3 | Servo 4
|
||||
TIM5 | RC In 4 | | |
|
||||
TIM6 | ----------- PIOS_PWM (Supervisor) ----------
|
||||
TIM7 | | | |
|
||||
TIM8 | Servo 5 | Servo 6 | Servo 7 | Servo 8
|
||||
------+-----------+-----------+-----------+----------
|
||||
*/
|
||||
|
||||
//------------------------
|
||||
// DMA Channels Used
|
||||
//------------------------
|
||||
/* Channel 1 - ADC */
|
||||
/* Channel 2 - SPI1 RX */
|
||||
/* Channel 3 - SPI1 TX */
|
||||
/* Channel 4 - SPI2 RX */
|
||||
/* Channel 5 - SPI2 TX */
|
||||
/* Channel 6 - */
|
||||
/* Channel 7 - */
|
||||
/* Channel 8 - */
|
||||
/* Channel 9 - */
|
||||
/* Channel 10 - */
|
||||
/* Channel 11 - */
|
||||
/* Channel 12 - */
|
||||
|
||||
//------------------------
|
||||
// PIOS_LED
|
||||
//------------------------
|
||||
#define PIOS_LED_LED1_GPIO_PORT GPIOC
|
||||
#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12
|
||||
#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_LED_LED2_GPIO_PORT GPIOC
|
||||
#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13
|
||||
#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_LED_NUM 2
|
||||
#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT }
|
||||
#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN }
|
||||
#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK }
|
||||
|
||||
//------------------------
|
||||
// PIOS_I2C
|
||||
//------------------------
|
||||
#define PIOS_I2C_PORT I2C2
|
||||
#define PIOS_I2C_CLK RCC_APB1Periph_I2C2
|
||||
#define PIOS_I2C_GPIO_PORT GPIOB
|
||||
#define PIOS_I2C_SDA_PIN GPIO_Pin_11
|
||||
#define PIOS_I2C_SCL_PIN GPIO_Pin_10
|
||||
#define PIOS_I2C_DUTY_CYCLE I2C_DutyCycle_2
|
||||
#define PIOS_I2C_BUS_FREQ 400000 // bit/s
|
||||
#define PIOS_I2C_TIMEOUT_VALUE 50 // ms
|
||||
#define PIOS_I2C_IRQ_EV_HANDLER void I2C2_EV_IRQHandler(void)
|
||||
#define PIOS_I2C_IRQ_ER_HANDLER void I2C2_ER_IRQHandler(void)
|
||||
#define PIOS_I2C_IRQ_EV_CHANNEL I2C2_EV_IRQn
|
||||
#define PIOS_I2C_IRQ_ER_CHANNEL I2C2_ER_IRQn
|
||||
#define PIOS_I2C_IRQ_EV_PRIORITY PIOS_IRQ_PRIO_HIGH
|
||||
#define PIOS_I2C_IRQ_ER_PRIORITY PIOS_IRQ_PRIO_HIGH
|
||||
|
||||
//------------------------
|
||||
// PIOS_BMP085
|
||||
//------------------------
|
||||
#define PIOS_BMP085_EOC_GPIO_PORT GPIOC
|
||||
#define PIOS_BMP085_EOC_GPIO_PIN GPIO_Pin_15
|
||||
#define PIOS_BMP085_EOC_PORT_SOURCE GPIO_PortSourceGPIOG
|
||||
#define PIOS_BMP085_EOC_PIN_SOURCE GPIO_PinSource8
|
||||
#define PIOS_BMP085_EOC_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_BMP085_EOC_EXTI_LINE EXTI_Line15
|
||||
#define PIOS_BMP085_EOC_IRQn EXTI15_10_IRQn
|
||||
#define PIOS_BMP085_EOC_PRIO PIOS_IRQ_PRIO_HIGH
|
||||
#define PIOS_BMP085_OVERSAMPLING 2
|
||||
|
||||
//-------------------------
|
||||
// USART
|
||||
//
|
||||
// See also pios_board.c
|
||||
//-------------------------
|
||||
#define PIOS_USART_RX_BUFFER_SIZE 1024
|
||||
#define PIOS_USART_TX_BUFFER_SIZE 256
|
||||
|
||||
#define PIOS_COM_TELEM_RF 0
|
||||
#define PIOS_COM_GPS 1
|
||||
#define PIOS_COM_TELEM_USB 2
|
||||
|
||||
#ifdef PIOS_ENABLE_AUX_UART
|
||||
#define PIOS_COM_AUX 3
|
||||
#define PIOS_COM_DEBUG PIOS_COM_AUX
|
||||
#endif
|
||||
|
||||
//-------------------------
|
||||
// SPI
|
||||
//
|
||||
// See also pios_board.c
|
||||
//-------------------------
|
||||
#define PIOS_SDCARD_SPI 0
|
||||
#define PIOS_OPAHRS_SPI 1
|
||||
|
||||
//-------------------------
|
||||
// Delay Timer
|
||||
//-------------------------
|
||||
#define PIOS_DELAY_TIMER TIM2
|
||||
#define PIOS_DELAY_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE)
|
||||
|
||||
//-------------------------
|
||||
// System Settings
|
||||
//-------------------------
|
||||
#define PIOS_MASTER_CLOCK 72000000
|
||||
#define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2)
|
||||
#if defined(USE_BOOTLOADER)
|
||||
#define PIOS_NVIC_VECTTAB_FLASH ((uint32_t)0x08006000)
|
||||
#else
|
||||
#define PIOS_NVIC_VECTTAB_FLASH ((uint32_t)0x08000000)
|
||||
#endif
|
||||
|
||||
//-------------------------
|
||||
// Interrupt Priorities
|
||||
//-------------------------
|
||||
#define PIOS_IRQ_PRIO_LOW 12 // lower than RTOS
|
||||
#define PIOS_IRQ_PRIO_MID 8 // higher than RTOS
|
||||
#define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc...
|
||||
#define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc...
|
||||
|
||||
//-------------------------
|
||||
// Receiver PWM inputs
|
||||
//-------------------------
|
||||
#define PIOS_PWM_CH1_GPIO_PORT GPIOA
|
||||
#define PIOS_PWM_CH1_PIN GPIO_Pin_9
|
||||
#define PIOS_PWM_CH1_TIM_PORT TIM1
|
||||
#define PIOS_PWM_CH1_CH TIM_Channel_2
|
||||
#define PIOS_PWM_CH1_CCR TIM_IT_CC2
|
||||
#define PIOS_PWM_CH2_GPIO_PORT GPIOA
|
||||
#define PIOS_PWM_CH2_PIN GPIO_Pin_10
|
||||
#define PIOS_PWM_CH2_TIM_PORT TIM1
|
||||
#define PIOS_PWM_CH2_CH TIM_Channel_3
|
||||
#define PIOS_PWM_CH2_CCR TIM_IT_CC3
|
||||
#define PIOS_PWM_CH3_GPIO_PORT GPIOA
|
||||
#define PIOS_PWM_CH3_PIN GPIO_Pin_0
|
||||
#define PIOS_PWM_CH3_TIM_PORT TIM5
|
||||
#define PIOS_PWM_CH3_CH TIM_Channel_1
|
||||
#define PIOS_PWM_CH3_CCR TIM_IT_CC1
|
||||
#define PIOS_PWM_CH4_GPIO_PORT GPIOA
|
||||
#define PIOS_PWM_CH4_PIN GPIO_Pin_8
|
||||
#define PIOS_PWM_CH4_TIM_PORT TIM1
|
||||
#define PIOS_PWM_CH4_CH TIM_Channel_1
|
||||
#define PIOS_PWM_CH4_CCR TIM_IT_CC1
|
||||
#define PIOS_PWM_CH5_GPIO_PORT GPIOB
|
||||
#define PIOS_PWM_CH5_PIN GPIO_Pin_1
|
||||
#define PIOS_PWM_CH5_TIM_PORT TIM3
|
||||
#define PIOS_PWM_CH5_CH TIM_Channel_4
|
||||
#define PIOS_PWM_CH5_CCR TIM_IT_CC4
|
||||
#define PIOS_PWM_CH6_GPIO_PORT GPIOB
|
||||
#define PIOS_PWM_CH6_PIN GPIO_Pin_0
|
||||
#define PIOS_PWM_CH6_TIM_PORT TIM3
|
||||
#define PIOS_PWM_CH6_CH TIM_Channel_3
|
||||
#define PIOS_PWM_CH6_CCR TIM_IT_CC3
|
||||
#define PIOS_PWM_CH7_GPIO_PORT GPIOB
|
||||
#define PIOS_PWM_CH7_PIN GPIO_Pin_4
|
||||
#define PIOS_PWM_CH7_TIM_PORT TIM3
|
||||
#define PIOS_PWM_CH7_CH TIM_Channel_1
|
||||
#define PIOS_PWM_CH7_CCR TIM_IT_CC1
|
||||
#define PIOS_PWM_CH8_GPIO_PORT GPIOB
|
||||
#define PIOS_PWM_CH8_PIN GPIO_Pin_5
|
||||
#define PIOS_PWM_CH8_TIM_PORT TIM3
|
||||
#define PIOS_PWM_CH8_CH TIM_Channel_2
|
||||
#define PIOS_PWM_CH8_CCR TIM_IT_CC2
|
||||
#define PIOS_PWM_GPIO_PORTS { PIOS_PWM_CH1_GPIO_PORT, PIOS_PWM_CH2_GPIO_PORT, PIOS_PWM_CH3_GPIO_PORT, PIOS_PWM_CH4_GPIO_PORT, PIOS_PWM_CH5_GPIO_PORT, PIOS_PWM_CH6_GPIO_PORT, PIOS_PWM_CH7_GPIO_PORT, PIOS_PWM_CH8_GPIO_PORT }
|
||||
#define PIOS_PWM_GPIO_PINS { PIOS_PWM_CH1_PIN, PIOS_PWM_CH2_PIN, PIOS_PWM_CH3_PIN, PIOS_PWM_CH4_PIN, PIOS_PWM_CH5_PIN, PIOS_PWM_CH6_PIN, PIOS_PWM_CH7_PIN, PIOS_PWM_CH8_PIN }
|
||||
#define PIOS_PWM_TIM_PORTS { PIOS_PWM_CH1_TIM_PORT, PIOS_PWM_CH2_TIM_PORT, PIOS_PWM_CH3_TIM_PORT, PIOS_PWM_CH4_TIM_PORT, PIOS_PWM_CH5_TIM_PORT, PIOS_PWM_CH6_TIM_PORT, PIOS_PWM_CH7_TIM_PORT, PIOS_PWM_CH8_TIM_PORT }
|
||||
#define PIOS_PWM_TIM_CHANNELS { PIOS_PWM_CH1_CH, PIOS_PWM_CH2_CH, PIOS_PWM_CH3_CH, PIOS_PWM_CH4_CH, PIOS_PWM_CH5_CH, PIOS_PWM_CH6_CH, PIOS_PWM_CH7_CH, PIOS_PWM_CH8_CH }
|
||||
#define PIOS_PWM_TIM_CCRS { PIOS_PWM_CH1_CCR, PIOS_PWM_CH2_CCR, PIOS_PWM_CH3_CCR, PIOS_PWM_CH4_CCR, PIOS_PWM_CH5_CCR, PIOS_PWM_CH6_CCR, PIOS_PWM_CH7_CCR, PIOS_PWM_CH8_CCR }
|
||||
#define PIOS_PWM_TIMS { TIM1, TIM3, TIM5 }
|
||||
#define PIOS_PWM_TIM_IRQS { TIM1_CC_IRQn, TIM3_IRQn, TIM5_IRQn }
|
||||
#define PIOS_PWM_NUM_INPUTS 8
|
||||
#define PIOS_PWM_NUM_TIMS 3
|
||||
#define PIOS_PWM_SUPV_ENABLED 1
|
||||
#define PIOS_PWM_SUPV_TIMER TIM6
|
||||
#define PIOS_PWM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
|
||||
#define PIOS_PWM_SUPV_HZ 25
|
||||
#define PIOS_PWM_SUPV_IRQ_CHANNEL TIM6_IRQn
|
||||
#define PIOS_PWM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
|
||||
|
||||
//-------------------------
|
||||
// Receiver PPM input
|
||||
//-------------------------
|
||||
#define PIOS_PPM_GPIO_PORT PIOS_PWM_CH1_GPIO_PORT
|
||||
#define PIOS_PPM_GPIO_PIN PIOS_PWM_CH1_PIN
|
||||
#define PIOS_PPM_TIM_PORT PIOS_PWM_CH1_TIM_PORT
|
||||
#define PIOS_PPM_TIM_CHANNEL PIOS_PWM_CH1_CH
|
||||
#define PIOS_PPM_TIM_CCR PIOS_PWM_CH1_CCR
|
||||
#define PIOS_PPM_TIM TIM1
|
||||
#define PIOS_PPM_TIM_IRQ TIM1_CC_IRQn
|
||||
#define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed
|
||||
#define PIOS_PPM_SUPV_ENABLED 1
|
||||
#define PIOS_PPM_SUPV_TIMER TIM6
|
||||
#define PIOS_PPM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE)
|
||||
#define PIOS_PPM_SUPV_HZ 25
|
||||
#define PIOS_PPM_SUPV_IRQ_CHANNEL TIM6_IRQn
|
||||
#define PIOS_PPM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)
|
||||
|
||||
|
||||
//-------------------------
|
||||
// Servo outputs
|
||||
//-------------------------
|
||||
#define PIOS_SERVO_GPIO_PORT_1TO4 GPIOB
|
||||
#define PIOS_SERVO_GPIO_PIN_1 GPIO_Pin_6
|
||||
#define PIOS_SERVO_GPIO_PIN_2 GPIO_Pin_7
|
||||
#define PIOS_SERVO_GPIO_PIN_3 GPIO_Pin_8
|
||||
#define PIOS_SERVO_GPIO_PIN_4 GPIO_Pin_9
|
||||
#define PIOS_SERVO_GPIO_PORT_5TO8 GPIOC
|
||||
#define PIOS_SERVO_GPIO_PIN_5 GPIO_Pin_6
|
||||
#define PIOS_SERVO_GPIO_PIN_6 GPIO_Pin_7
|
||||
#define PIOS_SERVO_GPIO_PIN_7 GPIO_Pin_8
|
||||
#define PIOS_SERVO_GPIO_PIN_8 GPIO_Pin_9
|
||||
#define PIOS_SERVO_NUM_OUTPUTS 8
|
||||
#define PIOS_SERVO_NUM_TIMERS PIOS_SERVO_NUM_OUTPUTS
|
||||
#define PIOS_SERVO_UPDATE_HZ 50
|
||||
#define PIOS_SERVOS_INITIAL_POSITION 1500
|
||||
|
||||
//-------------------------
|
||||
// ADC
|
||||
// PIOS_ADC_PinGet(0) = Temperature Sensor (On-board)
|
||||
// PIOS_ADC_PinGet(1) = Power Sensor (Current)
|
||||
// PIOS_ADC_PinGet(2) = Power Sensor (Voltage)
|
||||
// PIOS_ADC_PinGet(3) = On-board 5v Rail Sensor
|
||||
// PIOS_ADC_PinGet(4) = Auxiliary Input 1
|
||||
// PIOS_ADC_PinGet(5) = Auxiliary Input 2
|
||||
// PIOS_ADC_PinGet(6) = Auxiliary Input 3
|
||||
//-------------------------
|
||||
//#define PIOS_ADC_OVERSAMPLING_RATE 1
|
||||
#define PIOS_ADC_USE_TEMP_SENSOR 1
|
||||
#define PIOS_ADC_TEMP_SENSOR_ADC ADC1
|
||||
#define PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL 1
|
||||
#define PIOS_ADC_PIN1_GPIO_PORT GPIOA // PA1 (Power Sense - Voltage)
|
||||
#define PIOS_ADC_PIN1_GPIO_PIN GPIO_Pin_1 // ADC123_IN1
|
||||
#define PIOS_ADC_PIN1_GPIO_CHANNEL ADC_Channel_1
|
||||
#define PIOS_ADC_PIN1_ADC ADC1
|
||||
#define PIOS_ADC_PIN1_ADC_NUMBER 2
|
||||
#define PIOS_ADC_PIN2_GPIO_PORT GPIOC // PC3 (Power Sense - Current)
|
||||
#define PIOS_ADC_PIN2_GPIO_PIN GPIO_Pin_3 // ADC123_IN13
|
||||
#define PIOS_ADC_PIN2_GPIO_CHANNEL ADC_Channel_13
|
||||
#define PIOS_ADC_PIN2_ADC ADC2
|
||||
#define PIOS_ADC_PIN2_ADC_NUMBER 1
|
||||
#define PIOS_ADC_PIN3_GPIO_PORT GPIOC // PC5 (Onboard 5v Sensor) PC5
|
||||
#define PIOS_ADC_PIN3_GPIO_PIN GPIO_Pin_5 // ADC12_IN15
|
||||
#define PIOS_ADC_PIN3_GPIO_CHANNEL ADC_Channel_15
|
||||
#define PIOS_ADC_PIN3_ADC ADC2
|
||||
#define PIOS_ADC_PIN3_ADC_NUMBER 2
|
||||
#define PIOS_ADC_PIN4_GPIO_PORT GPIOC // PC0 (AUX 1)
|
||||
#define PIOS_ADC_PIN4_GPIO_PIN GPIO_Pin_0 // ADC123_IN10
|
||||
#define PIOS_ADC_PIN4_GPIO_CHANNEL ADC_Channel_10
|
||||
#define PIOS_ADC_PIN4_ADC ADC1
|
||||
#define PIOS_ADC_PIN4_ADC_NUMBER 3
|
||||
#define PIOS_ADC_PIN5_GPIO_PORT GPIOC // PC1 (AUX 2)
|
||||
#define PIOS_ADC_PIN5_GPIO_PIN GPIO_Pin_1 // ADC123_IN11
|
||||
#define PIOS_ADC_PIN5_GPIO_CHANNEL ADC_Channel_11
|
||||
#define PIOS_ADC_PIN5_ADC ADC2
|
||||
#define PIOS_ADC_PIN5_ADC_NUMBER 3
|
||||
#define PIOS_ADC_PIN6_GPIO_PORT GPIOC // PC2 (AUX 3)
|
||||
#define PIOS_ADC_PIN6_GPIO_PIN GPIO_Pin_2 // ADC123_IN12
|
||||
#define PIOS_ADC_PIN6_GPIO_CHANNEL ADC_Channel_12
|
||||
#define PIOS_ADC_PIN6_ADC ADC1
|
||||
#define PIOS_ADC_PIN6_ADC_NUMBER 4
|
||||
#define PIOS_ADC_NUM_PINS 6
|
||||
#define PIOS_ADC_PORTS { PIOS_ADC_PIN1_GPIO_PORT, PIOS_ADC_PIN2_GPIO_PORT, PIOS_ADC_PIN3_GPIO_PORT, PIOS_ADC_PIN4_GPIO_PORT, PIOS_ADC_PIN5_GPIO_PORT, PIOS_ADC_PIN6_GPIO_PORT }
|
||||
#define PIOS_ADC_PINS { PIOS_ADC_PIN1_GPIO_PIN, PIOS_ADC_PIN2_GPIO_PIN, PIOS_ADC_PIN3_GPIO_PIN, PIOS_ADC_PIN4_GPIO_PIN, PIOS_ADC_PIN5_GPIO_PIN, PIOS_ADC_PIN6_GPIO_PIN }
|
||||
#define PIOS_ADC_CHANNELS { PIOS_ADC_PIN1_GPIO_CHANNEL, PIOS_ADC_PIN2_GPIO_CHANNEL, PIOS_ADC_PIN3_GPIO_CHANNEL, PIOS_ADC_PIN4_GPIO_CHANNEL, PIOS_ADC_PIN5_GPIO_CHANNEL, PIOS_ADC_PIN6_GPIO_CHANNEL }
|
||||
#define PIOS_ADC_MAPPING { PIOS_ADC_PIN1_ADC, PIOS_ADC_PIN2_ADC, PIOS_ADC_PIN3_ADC, PIOS_ADC_PIN4_ADC, PIOS_ADC_PIN5_ADC, PIOS_ADC_PIN6_ADC }
|
||||
#define PIOS_ADC_CHANNEL_MAPPING { PIOS_ADC_PIN1_ADC_NUMBER, PIOS_ADC_PIN2_ADC_NUMBER, PIOS_ADC_PIN3_ADC_NUMBER, PIOS_ADC_PIN4_ADC_NUMBER, PIOS_ADC_PIN5_ADC_NUMBER, PIOS_ADC_PIN6_ADC_NUMBER }
|
||||
#define PIOS_ADC_NUM_CHANNELS (PIOS_ADC_NUM_PINS + PIOS_ADC_USE_TEMP_SENSOR)
|
||||
#define PIOS_ADC_NUM_ADC_CHANNELS 2
|
||||
#define PIOS_ADC_USE_ADC2 1
|
||||
#define PIOS_ADC_CLOCK_FUNCTION RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE)
|
||||
#define PIOS_ADC_ADCCLK RCC_PCLK2_Div8
|
||||
/* RCC_PCLK2_Div2: ADC clock = PCLK2/2 */
|
||||
/* RCC_PCLK2_Div4: ADC clock = PCLK2/4 */
|
||||
/* RCC_PCLK2_Div6: ADC clock = PCLK2/6 */
|
||||
/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */
|
||||
#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5
|
||||
/* Sample time: */
|
||||
/* With an ADCCLK = 14 MHz and a sampling time of 293.5 cycles: */
|
||||
/* Tconv = 239.5 + 12.5 = 252 cycles = 18<31>s */
|
||||
/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>S) */
|
||||
#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_HIGH
|
||||
|
||||
//-------------------------
|
||||
// GPIO
|
||||
//-------------------------
|
||||
#define PIOS_GPIO_1_PORT GPIOC
|
||||
#define PIOS_GPIO_1_PIN GPIO_Pin_0
|
||||
#define PIOS_GPIO_1_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_GPIO_2_PORT GPIOC
|
||||
#define PIOS_GPIO_2_PIN GPIO_Pin_1
|
||||
#define PIOS_GPIO_2_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_GPIO_3_PORT GPIOC
|
||||
#define PIOS_GPIO_3_PIN GPIO_Pin_2
|
||||
#define PIOS_GPIO_3_GPIO_CLK RCC_APB2Periph_GPIOC
|
||||
#define PIOS_GPIO_4_PORT GPIOD
|
||||
#define PIOS_GPIO_4_PIN GPIO_Pin_2
|
||||
#define PIOS_GPIO_4_GPIO_CLK RCC_APB2Periph_GPIOD
|
||||
#define PIOS_GPIO_PORTS { PIOS_GPIO_1_PORT, PIOS_GPIO_2_PORT, PIOS_GPIO_3_PORT, PIOS_GPIO_4_PORT }
|
||||
#define PIOS_GPIO_PINS { PIOS_GPIO_1_PIN, PIOS_GPIO_2_PIN, PIOS_GPIO_3_PIN, PIOS_GPIO_4_PIN }
|
||||
#define PIOS_GPIO_CLKS { PIOS_GPIO_1_GPIO_CLK, PIOS_GPIO_2_GPIO_CLK, PIOS_GPIO_3_GPIO_CLK, PIOS_GPIO_4_GPIO_CLK }
|
||||
#define PIOS_GPIO_NUM 4
|
||||
|
||||
//-------------------------
|
||||
// USB
|
||||
//-------------------------
|
||||
#define PIOS_USB_ENABLED 1
|
||||
#define PIOS_USB_DETECT_GPIO_PORT GPIOC
|
||||
#define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_4
|
||||
#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4
|
||||
#define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID
|
||||
|
||||
/**
|
||||
* glue macros for file IO
|
||||
* STM32 uses DOSFS for file IO
|
||||
*/
|
||||
#define PIOS_FOPEN_READ(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_READ, PIOS_SDCARD_Sector, &file) != DFS_OK
|
||||
|
||||
#define PIOS_FOPEN_WRITE(filename,file) DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, DFS_WRITE, PIOS_SDCARD_Sector, &file) != DFS_OK
|
||||
|
||||
#define PIOS_FREAD(file,bufferadr,length,resultadr) DFS_ReadFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length) != DFS_OK
|
||||
|
||||
#define PIOS_FWRITE(file,bufferadr,length,resultadr) DFS_WriteFile(file, PIOS_SDCARD_Sector, (uint8_t*)bufferadr, resultadr, length)
|
||||
|
||||
#define PIOS_FCLOSE(file) DFS_Close(&file)
|
||||
|
||||
#define PIOS_FUNLINK(filename) DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)filename, PIOS_SDCARD_Sector)
|
||||
|
||||
|
||||
|
||||
#endif /* PIOS_BOARD_H */
|
@ -1,73 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief PiOS configuration header.
|
||||
* Central compile time config for the project.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIOS_CONFIG_H
|
||||
#define PIOS_CONFIG_H
|
||||
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
#define PIOS_INCLUDE_LED
|
||||
//#define PIOS_INCLUDE_SPEKTRUM
|
||||
//#define PIOS_INCLUDE_PPM
|
||||
#define PIOS_INCLUDE_PWM
|
||||
#define PIOS_INCLUDE_SERVO
|
||||
#define PIOS_INCLUDE_SPI
|
||||
#define PIOS_INCLUDE_SYS
|
||||
#define PIOS_INCLUDE_USART
|
||||
//#define PIOS_INCLUDE_USB_COM
|
||||
#define PIOS_INCLUDE_USB_HID
|
||||
#define PIOS_INCLUDE_USB
|
||||
#define PIOS_INCLUDE_BMP085
|
||||
#define PIOS_INCLUDE_OPAHRS
|
||||
#define PIOS_INCLUDE_COM
|
||||
#define PIOS_INCLUDE_SDCARD
|
||||
#define PIOS_INCLUDE_SETTINGS
|
||||
#define PIOS_INCLUDE_FREERTOS
|
||||
#define PIOS_INCLUDE_GPIO
|
||||
#define PIOS_INCLUDE_EXTI
|
||||
|
||||
|
||||
/* Defaults for Logging */
|
||||
#define LOG_FILENAME "PIOS.LOG"
|
||||
#define STARTUP_LOG_ENABLED 1
|
||||
|
||||
/* COM Module */
|
||||
#define GPS_BAUDRATE 19200
|
||||
#define TELEM_BAUDRATE 19200
|
||||
#define AUXUART_ENABLED 0
|
||||
#define AUXUART_BAUDRATE 19200
|
||||
|
||||
/* Servos */
|
||||
#define SERVOS_POSITION_MIN 800
|
||||
#define SERVOS_POSITION_MAX 2200
|
||||
|
||||
|
||||
#endif /* PIOS_CONFIG_H */
|
@ -1,325 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up and runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
#include "uavobjectsinit.h"
|
||||
#include "systemmod.h"
|
||||
|
||||
|
||||
/* Task Priorities */
|
||||
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
#define INCLUDE_TEST_TASKS 0
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static uint8_t sdcard_available;
|
||||
#endif
|
||||
FILEINFO File;
|
||||
char Buffer[1024];
|
||||
uint32_t Cache;
|
||||
|
||||
/* Function Prototypes */
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskTick(void *pvParameters);
|
||||
static void TaskTesting(void *pvParameters);
|
||||
static void TaskHIDTest(void *pvParameters);
|
||||
static void TaskServos(void *pvParameters);
|
||||
static void TaskSDCard(void *pvParameters);
|
||||
#endif
|
||||
int32_t CONSOLE_Parse(uint8_t port, char c);
|
||||
void OP_ADC_NotifyChange(uint32_t pin, uint32_t pin_value);
|
||||
|
||||
/* Prototype of generated InitModules() function */
|
||||
extern void InitModules(void);
|
||||
|
||||
/* Prototype of PIOS_Board_Init() function */
|
||||
extern void PIOS_Board_Init(void);
|
||||
|
||||
/**
|
||||
* OpenPilot Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
/* NOTE: Do NOT modify the following start-up sequence */
|
||||
/* Any new initialization functions should be added in OpenPilotInit() */
|
||||
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Initialize the system thread */
|
||||
SystemModInitialize();
|
||||
|
||||
/* Start the FreeRTOS scheduler */
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
/* If we do get here, it will most likely be because we ran out of heap space. */
|
||||
PIOS_LED_Off(LED1);
|
||||
PIOS_LED_Off(LED2);
|
||||
for(;;) {
|
||||
PIOS_LED_Toggle(LED1);
|
||||
PIOS_LED_Toggle(LED2);
|
||||
PIOS_DELAY_WaitmS(100);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the hardware, libraries and modules (called by the System thread in systemmod.c)
|
||||
*/
|
||||
void OpenPilotInit()
|
||||
{
|
||||
|
||||
/* Architecture dependant Hardware and
|
||||
* core subsystem initialisation
|
||||
* (see pios_board.c for your arch)
|
||||
* */
|
||||
PIOS_Board_Init();
|
||||
|
||||
/* Initialize modules */
|
||||
InitModules();
|
||||
|
||||
/* Create test tasks */
|
||||
//xTaskCreate(TaskTesting, (signed portCHAR *)"Testing", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
|
||||
//xTaskCreate(TaskHIDTest, (signed portCHAR *)"HIDTest", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
|
||||
//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
|
||||
}
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskTesting(void *pvParameters)
|
||||
{
|
||||
portTickType xDelay = 250 / portTICK_RATE_MS;
|
||||
portTickType xTimeout = 10 / portTICK_RATE_MS;
|
||||
|
||||
//PIOS_BMP085_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
/* This blocks the task until the BMP085 EOC */
|
||||
/*
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetTemperature());
|
||||
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u\r", PIOS_BMP085_GetPressure());
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_SPEKTRUM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u\r", PIOS_SPEKTRUM_Get(0), PIOS_SPEKTRUM_Get(1), PIOS_SPEKTRUM_Get(2), PIOS_SPEKTRUM_Get(3), PIOS_SPEKTRUM_Get(4), PIOS_SPEKTRUM_Get(5), PIOS_SPEKTRUM_Get(6), PIOS_SPEKTRUM_Get(7));
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PWM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PWM_Get(0), PIOS_PWM_Get(1), PIOS_PWM_Get(2), PIOS_PWM_Get(3), PIOS_PWM_Get(4), PIOS_PWM_Get(5), PIOS_PWM_Get(6), PIOS_PWM_Get(7));
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%u,%u,%u,%u,%u,%u,%u,%u uS\r", PIOS_PPM_Get(0), PIOS_PPM_Get(1), PIOS_PPM_Get(2), PIOS_PPM_Get(3), PIOS_PPM_Get(4), PIOS_PPM_Get(5), PIOS_PPM_Get(6), PIOS_PPM_Get(7));
|
||||
#endif
|
||||
|
||||
/* This blocks the task until there is something on the buffer */
|
||||
/*xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
|
||||
int32_t len = PIOS_COM_ReceiveBufferUsed(COM_USART1);
|
||||
for(int32_t i = 0; i < len; i++) {
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, ">%c\r", PIOS_COM_ReceiveBuffer(COM_USART1));
|
||||
}*/
|
||||
|
||||
//int32_t state = PIOS_USB_CableConnected();
|
||||
//PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "State: %d\r", state);
|
||||
|
||||
//PIOS_I2C_Transfer(I2C_Write_WithoutStop, 0x57, (uint8_t *)50, 1);
|
||||
|
||||
/* Test ADC pins */
|
||||
//temp = ((1.43 - ((Vsense / 4096) * 3.3)) / 4.3) + 25;
|
||||
//uint32_t vsense = PIOS_ADC_PinGet(0);
|
||||
//uint32_t Temp = (1.42 - vsense * 3.3 / 4096) * 1000 / 4.35 + 25;
|
||||
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "Temp: %d, CS_I: %d, CS_V: %d, 5v: %d\r", PIOS_ADC_PinGet(0), PIOS_ADC_PinGet(1), PIOS_ADC_PinGet(2), PIOS_ADC_PinGet(3));
|
||||
//PIOS_COM_SendFormattedString(COM_DEBUG_USART, "AUX1: %d, AUX2: %d, AUX3: %d\r", PIOS_ADC_PinGet(4), PIOS_ADC_PinGet(5), PIOS_ADC_PinGet(6));
|
||||
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskHIDTest(void *pvParameters)
|
||||
{
|
||||
uint8_t byte;
|
||||
uint8_t line_buffer[128];
|
||||
uint16_t line_ix = 0;
|
||||
|
||||
for(;;)
|
||||
{
|
||||
/* HID Loopback Test */
|
||||
#if 0
|
||||
if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) != 0) {
|
||||
byte = PIOS_COM_ReceiveBuffer(COM_USB_HID);
|
||||
if(byte == '\r' || byte == '\n' || byte == 0) {
|
||||
PIOS_COM_SendFormattedString(COM_USB_HID, "RX: %s\r", line_buffer);
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
|
||||
line_ix = 0;
|
||||
} else if(line_ix < sizeof(line_buffer)) {
|
||||
line_buffer[line_ix++] = byte;
|
||||
line_buffer[line_ix] = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* HID Loopback Test */
|
||||
if(PIOS_COM_ReceiveBufferUsed(COM_USART2) != 0) {
|
||||
byte = PIOS_COM_ReceiveBuffer(COM_USART2);
|
||||
#if 0
|
||||
if(byte == '\r' || byte == '\n' || byte == 0) {
|
||||
PIOS_COM_SendFormattedString(COM_DEBUG_USART, "RX: %s\r", line_buffer);
|
||||
line_ix = 0;
|
||||
} else if(line_ix < sizeof(line_buffer)) {
|
||||
line_buffer[line_ix++] = byte;
|
||||
line_buffer[line_ix] = 0;
|
||||
}
|
||||
#endif
|
||||
PIOS_COM_SendChar(COM_DEBUG_USART, (char)byte);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskServos(void *pvParameters)
|
||||
{
|
||||
/* For testing servo outputs */
|
||||
portTickType xDelay;
|
||||
|
||||
/* Used to test servos, cycles all servos from one side to the other */
|
||||
for(;;) {
|
||||
/*xDelay = 250 / portTICK_RATE_MS;
|
||||
PIOS_Servo_Set(0, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(1, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(2, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(3, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(4, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(5, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(6, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(7, 2000);
|
||||
vTaskDelay(xDelay);
|
||||
|
||||
PIOS_Servo_Set(7, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(6, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(5, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(4, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(3, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(2, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(1, 1000);
|
||||
vTaskDelay(xDelay);
|
||||
PIOS_Servo_Set(0, 1000);
|
||||
vTaskDelay(xDelay);*/
|
||||
|
||||
xDelay = 1 / portTICK_RATE_MS;
|
||||
for(int i = 1000; i < 2000; i++) {
|
||||
PIOS_Servo_Set(0, i);
|
||||
PIOS_Servo_Set(1, i);
|
||||
PIOS_Servo_Set(2, i);
|
||||
PIOS_Servo_Set(3, i);
|
||||
PIOS_Servo_Set(4, i);
|
||||
PIOS_Servo_Set(5, i);
|
||||
PIOS_Servo_Set(6, i);
|
||||
PIOS_Servo_Set(7, i);
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
for(int i = 2000; i > 1000; i--) {
|
||||
PIOS_Servo_Set(0, i);
|
||||
PIOS_Servo_Set(1, i);
|
||||
PIOS_Servo_Set(2, i);
|
||||
PIOS_Servo_Set(3, i);
|
||||
PIOS_Servo_Set(4, i);
|
||||
PIOS_Servo_Set(5, i);
|
||||
PIOS_Servo_Set(6, i);
|
||||
PIOS_Servo_Set(7, i);
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if INCLUDE_TEST_TASKS
|
||||
static void TaskSDCard(void *pvParameters)
|
||||
{
|
||||
uint16_t second_delay_ctr = 0;
|
||||
portTickType xLastExecutionTime;
|
||||
|
||||
/* Initialise the xLastExecutionTime variable on task entry */
|
||||
xLastExecutionTime = xTaskGetTickCount();
|
||||
|
||||
for(;;) {
|
||||
vTaskDelayUntil(&xLastExecutionTime, 1 / portTICK_RATE_MS);
|
||||
|
||||
/* Each second: */
|
||||
/* Check if SD card is available */
|
||||
/* High-speed access if SD card was previously available */
|
||||
if(++second_delay_ctr >= 1000) {
|
||||
second_delay_ctr = 0;
|
||||
|
||||
uint8_t prev_sdcard_available = sdcard_available;
|
||||
sdcard_available = PIOS_SDCARD_CheckAvailable(prev_sdcard_available);
|
||||
|
||||
if(sdcard_available && !prev_sdcard_available) {
|
||||
/* SD Card has been connected! */
|
||||
/* Switch to mass storage device */
|
||||
MSD_Init(0);
|
||||
} else if(!sdcard_available && prev_sdcard_available) {
|
||||
/* Re-init USB for HID */
|
||||
PIOS_USB_Init(1);
|
||||
/* SD Card disconnected! */
|
||||
}
|
||||
}
|
||||
|
||||
/* Each millisecond: */
|
||||
/* Handle USB access if device is available */
|
||||
if(sdcard_available) {
|
||||
MSD_Periodic_mS();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -1,501 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file pios_board.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Defines board specific static initializers for hardware for the OpenPilot board.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_spi_priv.h>
|
||||
#include <pios_usart_priv.h>
|
||||
#include <pios_com_priv.h>
|
||||
#include <openpilot.h>
|
||||
#include <uavobjectsinit.h>
|
||||
|
||||
/**
|
||||
* PIOS_Board_Init()
|
||||
* initializes all the core subsystems on this specific hardware
|
||||
* called from System/openpilot.c
|
||||
*/
|
||||
void PIOS_Board_Init(void) {
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* SPI Init */
|
||||
PIOS_SPI_Init();
|
||||
|
||||
/* Enable and mount the SDCard */
|
||||
PIOS_SDCARD_Init();
|
||||
PIOS_SDCARD_MountFS(0);
|
||||
|
||||
/* Initialize UAVObject libraries */
|
||||
EventDispatcherInitialize();
|
||||
UAVObjInitialize();
|
||||
UAVObjectsInitializeAll();
|
||||
|
||||
/* Initialize the alarms library */
|
||||
AlarmsInitialize();
|
||||
|
||||
/* Initialize the PiOS library */
|
||||
PIOS_COM_Init();
|
||||
PIOS_Servo_Init();
|
||||
PIOS_ADC_Init();
|
||||
PIOS_GPIO_Init();
|
||||
#if defined(PIOS_INCLUDE_SPEKTRUM)
|
||||
PIOS_SPEKTRUM_Init();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PWM)
|
||||
PIOS_PWM_Init();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_PPM)
|
||||
PIOS_PPM_Init();
|
||||
#endif
|
||||
PIOS_USB_Init(0);
|
||||
PIOS_I2C_Init();
|
||||
|
||||
}
|
||||
|
||||
/* MicroSD Interface
|
||||
*
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_sdcard_irq_handler(void);
|
||||
void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
|
||||
void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler")));
|
||||
const struct pios_spi_cfg pios_spi_sdcard_cfg = {
|
||||
.regs = SPI1,
|
||||
.init = {
|
||||
.SPI_Mode = SPI_Mode_Master,
|
||||
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
|
||||
.SPI_DataSize = SPI_DataSize_8b,
|
||||
.SPI_NSS = SPI_NSS_Soft,
|
||||
.SPI_FirstBit = SPI_FirstBit_MSB,
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
.SPI_BaudRatePrescaler = 7 << 3, /* Maximum divider (ie. slowest clock rate) */
|
||||
},
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.handler = PIOS_SPI_sdcard_irq_handler,
|
||||
.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel2_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel2,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel3,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_4,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_5,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/* AHRS Interface
|
||||
*
|
||||
* NOTE: Leave this declared as const data so that it ends up in the
|
||||
* .rodata section (ie. Flash) rather than in the .bss section (RAM).
|
||||
*/
|
||||
void PIOS_SPI_ahrs_irq_handler(void);
|
||||
void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
|
||||
void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler")));
|
||||
const struct pios_spi_cfg pios_spi_ahrs_cfg = {
|
||||
.regs = SPI2,
|
||||
.init = {
|
||||
.SPI_Mode = SPI_Mode_Master,
|
||||
.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
|
||||
.SPI_DataSize = SPI_DataSize_8b,
|
||||
.SPI_NSS = SPI_NSS_Soft,
|
||||
.SPI_FirstBit = SPI_FirstBit_MSB,
|
||||
.SPI_CRCPolynomial = 7,
|
||||
.SPI_CPOL = SPI_CPOL_High,
|
||||
.SPI_CPHA = SPI_CPHA_2Edge,
|
||||
.SPI_BaudRatePrescaler = 7 << 3, /* Maximum divider (ie. slowest clock rate) */
|
||||
},
|
||||
.use_crc = TRUE,
|
||||
.dma = {
|
||||
.ahb_clk = RCC_AHBPeriph_DMA1,
|
||||
|
||||
.irq = {
|
||||
.handler = PIOS_SPI_ahrs_irq_handler,
|
||||
.flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA1_Channel4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
|
||||
.rx = {
|
||||
.channel = DMA1_Channel4,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralSRC,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.channel = DMA1_Channel5,
|
||||
.init = {
|
||||
.DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR),
|
||||
.DMA_DIR = DMA_DIR_PeripheralDST,
|
||||
.DMA_PeripheralInc = DMA_PeripheralInc_Disable,
|
||||
.DMA_MemoryInc = DMA_MemoryInc_Enable,
|
||||
.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
|
||||
.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte,
|
||||
.DMA_Mode = DMA_Mode_Normal,
|
||||
.DMA_Priority = DMA_Priority_Medium,
|
||||
.DMA_M2M = DMA_M2M_Disable,
|
||||
},
|
||||
},
|
||||
},
|
||||
.ssel = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_12,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_Out_PP,
|
||||
},
|
||||
},
|
||||
.sclk = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_13,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
.miso = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_14,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IN_FLOATING,
|
||||
},
|
||||
},
|
||||
.mosi = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_15,
|
||||
.GPIO_Speed = GPIO_Speed_50MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/*
|
||||
* Board specific number of devices.
|
||||
*/
|
||||
struct pios_spi_dev pios_spi_devs[] = {
|
||||
{
|
||||
.cfg = &pios_spi_sdcard_cfg,
|
||||
},
|
||||
{
|
||||
.cfg = &pios_spi_ahrs_cfg,
|
||||
},
|
||||
};
|
||||
|
||||
uint8_t pios_spi_num_devices = NELEMENTS(pios_spi_devs);
|
||||
|
||||
void PIOS_SPI_sdcard_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(PIOS_SDCARD_SPI);
|
||||
}
|
||||
|
||||
void PIOS_SPI_ahrs_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_SPI_IRQ_Handler(PIOS_OPAHRS_SPI);
|
||||
}
|
||||
|
||||
/*
|
||||
* Telemetry USART
|
||||
*/
|
||||
void PIOS_USART_telem_irq_handler(void);
|
||||
void USART2_IRQHandler() __attribute__ ((alias ("PIOS_USART_telem_irq_handler")));
|
||||
const struct pios_usart_cfg pios_usart_telem_cfg = {
|
||||
.regs = USART2,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.handler = PIOS_USART_telem_irq_handler,
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART2_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_3,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOA,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_2,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
/*
|
||||
* GPS USART
|
||||
*/
|
||||
void PIOS_USART_gps_irq_handler(void);
|
||||
void USART3_IRQHandler() __attribute__ ((alias ("PIOS_USART_gps_irq_handler")));
|
||||
const struct pios_usart_cfg pios_usart_gps_cfg = {
|
||||
.regs = USART3,
|
||||
.remap = GPIO_PartialRemap_USART3,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.handler = PIOS_USART_gps_irq_handler,
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART3_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_11,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOC,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_10,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
/*
|
||||
* AUX USART
|
||||
*/
|
||||
void PIOS_USART_aux_irq_handler(void);
|
||||
void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler")));
|
||||
const struct pios_usart_cfg pios_usart_aux_cfg = {
|
||||
.regs = USART1,
|
||||
.init = {
|
||||
.USART_BaudRate = 57600,
|
||||
.USART_WordLength = USART_WordLength_8b,
|
||||
.USART_Parity = USART_Parity_No,
|
||||
.USART_StopBits = USART_StopBits_1,
|
||||
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
|
||||
.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
|
||||
},
|
||||
.irq = {
|
||||
.handler = PIOS_USART_aux_irq_handler,
|
||||
.init = {
|
||||
.NVIC_IRQChannel = USART1_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.remap = GPIO_Remap_USART1,
|
||||
.rx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_7,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_IPU,
|
||||
},
|
||||
},
|
||||
.tx = {
|
||||
.gpio = GPIOB,
|
||||
.init = {
|
||||
.GPIO_Pin = GPIO_Pin_6,
|
||||
.GPIO_Speed = GPIO_Speed_2MHz,
|
||||
.GPIO_Mode = GPIO_Mode_AF_PP,
|
||||
},
|
||||
},
|
||||
};
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Board specific number of devices.
|
||||
*/
|
||||
struct pios_usart_dev pios_usart_devs[] = {
|
||||
#define PIOS_USART_TELEM 0
|
||||
{
|
||||
.cfg = &pios_usart_telem_cfg,
|
||||
},
|
||||
#define PIOS_USART_GPS 1
|
||||
{
|
||||
.cfg = &pios_usart_gps_cfg,
|
||||
},
|
||||
#ifdef PIOS_COM_AUX
|
||||
#define PIOS_USART_AUX 2
|
||||
{
|
||||
.cfg = &pios_usart_aux_cfg,
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
uint8_t pios_usart_num_devices = NELEMENTS(pios_usart_devs);
|
||||
|
||||
void PIOS_USART_telem_irq_handler(void)
|
||||
{
|
||||
PIOS_USART_IRQ_Handler(PIOS_USART_TELEM);
|
||||
}
|
||||
|
||||
void PIOS_USART_gps_irq_handler(void)
|
||||
{
|
||||
PIOS_USART_IRQ_Handler(PIOS_USART_GPS);
|
||||
}
|
||||
|
||||
#ifdef PIOS_COM_AUX
|
||||
void PIOS_USART_aux_irq_handler(void)
|
||||
{
|
||||
PIOS_USART_IRQ_Handler(PIOS_USART_AUX);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* COM devices
|
||||
*/
|
||||
|
||||
/*
|
||||
* Board specific number of devices.
|
||||
*/
|
||||
extern const struct pios_com_driver pios_usart_com_driver;
|
||||
extern const struct pios_com_driver pios_usb_com_driver;
|
||||
|
||||
struct pios_com_dev pios_com_devs[] = {
|
||||
{
|
||||
.id = PIOS_USART_TELEM,
|
||||
.driver = &pios_usart_com_driver,
|
||||
},
|
||||
{
|
||||
.id = PIOS_USART_GPS,
|
||||
.driver = &pios_usart_com_driver,
|
||||
},
|
||||
{
|
||||
.id = 0,
|
||||
.driver = &pios_usb_com_driver,
|
||||
},
|
||||
#ifdef PIOS_COM_AUX
|
||||
{
|
||||
.id = PIOS_USART_AUX,
|
||||
.driver = &pios_usart_com_driver,
|
||||
},
|
||||
#endif
|
||||
};
|
||||
|
||||
const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
|
||||
|
@ -1,100 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up and runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
/* Task Priorities */
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
|
||||
/* Function Prototypes */
|
||||
static void TaskTesting(void *pvParameters);
|
||||
|
||||
/**
|
||||
* Test Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
// Init
|
||||
PIOS_SYS_Init();
|
||||
PIOS_DELAY_Init();
|
||||
PIOS_COM_Init();
|
||||
PIOS_ADC_Init();
|
||||
PIOS_I2C_Init();
|
||||
xTaskCreate(TaskTesting, (signed portCHAR *)"Test", configMINIMAL_STACK_SIZE , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
|
||||
// Should not get here
|
||||
PIOS_DEBUG_Assert(0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void TaskTesting(void *pvParameters)
|
||||
{
|
||||
portTickType xDelay = 500 / portTICK_RATE_MS;
|
||||
portTickType xTimeout = 10 / portTICK_RATE_MS;
|
||||
|
||||
PIOS_BMP085_Init();
|
||||
|
||||
for(;;)
|
||||
{
|
||||
PIOS_LED_Toggle(LED2);
|
||||
|
||||
int16_t temp;
|
||||
int32_t pressure;
|
||||
|
||||
PIOS_BMP085_StartADC(TemperatureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
|
||||
PIOS_BMP085_StartADC(PressureConv);
|
||||
xSemaphoreTake(PIOS_BMP085_EOC, xTimeout);
|
||||
PIOS_BMP085_ReadADC();
|
||||
|
||||
temp = PIOS_BMP085_GetTemperature();
|
||||
pressure = PIOS_BMP085_GetPressure();
|
||||
|
||||
PIOS_COM_SendFormattedStringNonBlocking(COM_DEBUG_USART, "%3u,%4u\r", temp, pressure);
|
||||
|
||||
vTaskDelay(xDelay);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Idle hook function
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
|
||||
|
||||
}
|
@ -1,37 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_common.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up ans runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
/**
|
||||
* Called by the RTOS when a stack overflow is detected.
|
||||
*/
|
||||
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed portCHAR *pcTaskName )
|
||||
{
|
||||
PIOS_DEBUG_Panic("STACK OVERFLOW");
|
||||
}
|
||||
|
||||
|
@ -1,89 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_cpuload.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Calibration for the CPU load calculation
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* This file is used to calculate the number of counts we get when
|
||||
* the CPU is fully idle (no load). This is used by systemmod.c to
|
||||
* calculate the CPU load. This calibration should be run whenever
|
||||
* changes are made in the FreeRTOS configuration or on the compiler
|
||||
* optimisation parameters.
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
// Local constants
|
||||
#define BENCHMARK_DURATION_MS 10000
|
||||
|
||||
// Local functions
|
||||
static void testTask(void *pvParameters);
|
||||
|
||||
// Variables
|
||||
static uint32_t idleCounter = 0;
|
||||
static uint32_t idleCounterClear = 0;
|
||||
|
||||
int main()
|
||||
{
|
||||
PIOS_SYS_Init();
|
||||
|
||||
// Create test task
|
||||
xTaskCreate(testTask, (signed portCHAR *)"Test", 1000 , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void testTask(void *pvParameters)
|
||||
{
|
||||
uint32_t countsPerSecond = 0;
|
||||
|
||||
while (1)
|
||||
{
|
||||
// Delay until enough required test duration
|
||||
vTaskDelay( BENCHMARK_DURATION_MS / portTICK_RATE_MS );
|
||||
|
||||
// Calculate counts per second, set breakpoint here
|
||||
countsPerSecond = idleCounter / (BENCHMARK_DURATION_MS/1000);
|
||||
|
||||
// Reset and start again - do not clear idleCounter directly!
|
||||
// SET BREAKPOINT HERE and read the countsPerSecond variable
|
||||
// this should be used to update IDLE_COUNTS_PER_SEC_AT_NO_LOAD in systemmod.c
|
||||
idleCounterClear = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
if (idleCounterClear == 0)
|
||||
{
|
||||
++idleCounter;
|
||||
}
|
||||
else
|
||||
{
|
||||
idleCounter = 0;
|
||||
idleCounterClear = 0;
|
||||
}
|
||||
}
|
@ -1,282 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file openpilot.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Sets up ans runs main OpenPilot tasks.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* I2C Test: communicate with external PCF8570 ram chip
|
||||
* For this test to function, PCF8570 chips need to be attached to the I2C port
|
||||
*
|
||||
* Blinking Blue LED: No errors detected, test continues
|
||||
* Blinking Red LED: Error detected, test stopped
|
||||
*
|
||||
*/
|
||||
|
||||
/* OpenPilot Includes */
|
||||
#include "openpilot.h"
|
||||
|
||||
//#define USE_DEBUG_PINS
|
||||
|
||||
#define DEVICE_1_ADDRESS 0x50
|
||||
#define DEVICE_2_ADDRESS 0x51
|
||||
|
||||
#ifdef USE_DEBUG_PINS
|
||||
#define DEBUG_PIN_TASK1_WAIT 0
|
||||
#define DEBUG_PIN_TASK1_LOCKED 1
|
||||
#define DEBUG_PIN_TASK2_WAIT 2
|
||||
#define DEBUG_PIN_TASK2_LOCKED 3
|
||||
#define DEBUG_PIN_TRANSFER 4
|
||||
#define DEBUG_PIN_IDLE 6
|
||||
#define DEBUG_PIN_ERROR 7
|
||||
#define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
|
||||
#define DebugPinLow(x) PIOS_DEBUG_PinLow(x)
|
||||
#else
|
||||
#define DebugPinHigh(x)
|
||||
#define DebugPinLow(x)
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#define MAX_LOCK_WAIT 2 // Time in ms that a thread can normally block I2C
|
||||
|
||||
/* Task Priorities */
|
||||
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
|
||||
|
||||
|
||||
/* Global Variables */
|
||||
|
||||
/* Local Variables */
|
||||
|
||||
/* Function Prototypes */
|
||||
static void Task1(void *pvParameters);
|
||||
static void Task2(void *pvParameters);
|
||||
|
||||
|
||||
/**
|
||||
* OpenPilot Main function
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
// Init
|
||||
PIOS_SYS_Init();
|
||||
PIOS_DEBUG_Init();
|
||||
PIOS_DELAY_Init();
|
||||
PIOS_COM_Init();
|
||||
PIOS_USB_Init(0);
|
||||
PIOS_I2C_Init();
|
||||
|
||||
// Both leds off
|
||||
PIOS_LED_Off(LED1);
|
||||
PIOS_LED_Off(LED2);
|
||||
|
||||
// Create task
|
||||
xTaskCreate(Task1, (signed portCHAR *)"Task1", 1024 , NULL, 1, NULL);
|
||||
xTaskCreate(Task2, (signed portCHAR *)"Task2", 1024 , NULL, 2, NULL);
|
||||
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
|
||||
/* If all is well we will never reach here as the scheduler will now be running. */
|
||||
/* If we do get here, it will most likely be because we ran out of heap space. */
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void OnError(void)
|
||||
{
|
||||
PIOS_LED_Off(LED1);
|
||||
while(1)
|
||||
{
|
||||
DebugPinHigh(DEBUG_PIN_ERROR);
|
||||
PIOS_LED_Toggle(LED2);
|
||||
vTaskDelay(50 / portTICK_RATE_MS);
|
||||
DebugPinLow(DEBUG_PIN_ERROR);
|
||||
}
|
||||
}
|
||||
//
|
||||
// This is a low priority task that will continuously access one I2C device
|
||||
// Frequently it will release the I2C device so that others can also use I2C
|
||||
//
|
||||
static void Task1(void *pvParameters)
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20\xB0\xB1\xB2", 4) != 0)
|
||||
OnError();
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
for(;;)
|
||||
{
|
||||
i++;
|
||||
if (i==100)
|
||||
{
|
||||
PIOS_LED_Toggle(LED1);
|
||||
i = 0;
|
||||
}
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK1_WAIT);
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
uint8_t buf[20];
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK1_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK1_LOCKED);
|
||||
|
||||
// Write A0 A1 A2 at address 0x10
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10\xA0\xA1\xA2", 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
|
||||
// Read 3 bytes at address 0x20 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x20", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
if (memcmp(buf, "\xB0\xB1\xB2",3) != 0)
|
||||
OnError();
|
||||
|
||||
// Read 3 bytes at address 0x10 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_1_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_1_ADDRESS<<1, buf, 3) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
if (memcmp(buf, "\xA0\xA1\xA2",3) != 0)
|
||||
OnError();
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK1_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// This is a high priority task that will periodically perform some actions on the second I2C device
|
||||
// Most of the time it will have to wait for the other task task to release I2C
|
||||
static void Task2(void *pvParameters)
|
||||
{
|
||||
portTickType xLastExecutionTime;
|
||||
|
||||
xLastExecutionTime = xTaskGetTickCount();
|
||||
uint32_t count = 0;
|
||||
|
||||
for(;;)
|
||||
{
|
||||
uint8_t buf[20];
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
|
||||
if (PIOS_I2C_LockDevice(MAX_LOCK_WAIT / portTICK_RATE_MS))
|
||||
{
|
||||
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
|
||||
|
||||
// Write value of count to address 0x10
|
||||
buf[0] = 0x10; // The address
|
||||
memcpy(&buf[1], &count, 4); // The data to write
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write, DEVICE_2_ADDRESS<<1, buf, 5) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
vTaskDelay(2 / portTICK_RATE_MS);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_WAIT);
|
||||
if (PIOS_I2C_LockDevice(1 / portTICK_RATE_MS))
|
||||
{
|
||||
DebugPinLow(DEBUG_PIN_TASK2_WAIT);
|
||||
DebugPinHigh(DEBUG_PIN_TASK2_LOCKED);
|
||||
|
||||
// Read at address 0x10 and check
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Write_WithoutStop, DEVICE_2_ADDRESS<<1, (uint8_t*)"\x10", 1) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (PIOS_I2C_Transfer(I2C_Read, DEVICE_2_ADDRESS<<1, buf, 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
DebugPinHigh(DEBUG_PIN_TRANSFER);
|
||||
if (memcmp(buf, &count, 4) != 0)
|
||||
OnError();
|
||||
DebugPinLow(DEBUG_PIN_TRANSFER);
|
||||
|
||||
|
||||
DebugPinLow(DEBUG_PIN_TASK2_LOCKED);
|
||||
PIOS_I2C_UnlockDevice();
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError();
|
||||
}
|
||||
|
||||
vTaskDelay(5 / portTICK_RATE_MS);
|
||||
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Idle hook function
|
||||
*/
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
DebugPinHigh(DEBUG_PIN_IDLE);
|
||||
DebugPinLow(DEBUG_PIN_IDLE);
|
||||
}
|
@ -1,174 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file test_uavobjects.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Tests for the UAVObject libraries
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "exampleobject1.h"
|
||||
#include "uavobjectsinit.h"
|
||||
|
||||
// Local functions
|
||||
static void testTask(void *pvParameters);
|
||||
static void eventCallback(UAVObjEvent* ev);
|
||||
static void eventCallbackPeriodic(UAVObjEvent* ev);
|
||||
|
||||
// Variables
|
||||
int testDelay = 0;
|
||||
|
||||
int main()
|
||||
{
|
||||
/* Brings up System using CMSIS functions, enables the LEDs. */
|
||||
PIOS_SYS_Init();
|
||||
|
||||
/* Delay system */
|
||||
PIOS_DELAY_Init();
|
||||
|
||||
/* SPI Init */
|
||||
PIOS_SPI_Init();
|
||||
|
||||
/* Enables the SDCard */
|
||||
PIOS_SDCARD_Init();
|
||||
PIOS_SDCARD_MountFS(0);
|
||||
|
||||
// Turn on both LEDs
|
||||
PIOS_LED_On(LED1);
|
||||
PIOS_LED_On(LED2);
|
||||
|
||||
// Create test task
|
||||
xTaskCreate(testTask, (signed portCHAR *)"ObjTest", 1000 , NULL, 1, NULL);
|
||||
|
||||
// Start the FreeRTOS scheduler
|
||||
vTaskStartScheduler();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void testTask(void *pvParameters)
|
||||
{
|
||||
ExampleObject1Data data;
|
||||
|
||||
// Initialize object and manager
|
||||
EventDispatcherInitialize();
|
||||
UAVObjInitialize();
|
||||
UAVObjectsInitializeAll();
|
||||
|
||||
// Set data
|
||||
ExampleObject1Get(&data);
|
||||
data.Field1 = 1;
|
||||
data.Field2 = 2;
|
||||
ExampleObject1Set(&data);
|
||||
|
||||
// Get data
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Benchmark time is takes to get and set data (both operations)
|
||||
int tickStart = xTaskGetTickCount();
|
||||
for (int n = 0; n < 10000; ++n)
|
||||
{
|
||||
ExampleObject1Set(&data);
|
||||
ExampleObject1Get(&data);
|
||||
}
|
||||
int delay = xTaskGetTickCount() - tickStart;
|
||||
|
||||
// Create a new instance and get/set its data
|
||||
uint16_t instId = ExampleObject1CreateInstance();
|
||||
ExampleObject1InstSet(instId, &data);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1InstGet(instId, &data);
|
||||
|
||||
// Test pack/unpack
|
||||
uint8_t buffer[EXAMPLEOBJECT1_NUMBYTES];
|
||||
memset(buffer, 0, EXAMPLEOBJECT1_NUMBYTES);
|
||||
UAVObjPack(ExampleObject1Handle(), 0, buffer);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Set(&data);
|
||||
UAVObjUnpack(ExampleObject1Handle(), 0, buffer);
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Test object saving/loading to SD card
|
||||
UAVObjSave(ExampleObject1Handle(), 0);
|
||||
memset(&data, 0, sizeof(data));
|
||||
ExampleObject1Set(&data);
|
||||
UAVObjLoad(ExampleObject1Handle(), 0);
|
||||
ExampleObject1Get(&data);
|
||||
|
||||
// Retrieve object handle by ID
|
||||
UAVObjHandle handle = UAVObjGetByID(EXAMPLEOBJECT1_OBJID);
|
||||
const char* name = UAVObjGetName(handle);
|
||||
|
||||
// Get/Set the metadata
|
||||
UAVObjMetadata mdata;
|
||||
ExampleObject1GetMetadata(&mdata);
|
||||
mdata.gcsTelemetryAcked = 0;
|
||||
ExampleObject1SetMetadata(&mdata);
|
||||
memset(&mdata, 0, sizeof(mdata));
|
||||
ExampleObject1GetMetadata(&mdata);
|
||||
|
||||
// Test callbacks
|
||||
ExampleObject1ConnectCallback(eventCallback);
|
||||
ExampleObject1Set(&data);
|
||||
|
||||
// Test queue events
|
||||
xQueueHandle queue;
|
||||
queue = xQueueCreate(10, sizeof(UAVObjEvent));
|
||||
ExampleObject1ConnectQueue(queue);
|
||||
|
||||
// Test periodic events
|
||||
UAVObjEvent ev;
|
||||
ev.event = 0;
|
||||
ev.instId = 0;
|
||||
ev.obj = ExampleObject1Handle();
|
||||
EventPeriodicCallbackCreate(&ev, eventCallbackPeriodic, 500);
|
||||
EventPeriodicQueueCreate(&ev, queue, 250);
|
||||
|
||||
// Done testing
|
||||
while (1)
|
||||
{
|
||||
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
|
||||
{
|
||||
name = UAVObjGetName(ev.obj);
|
||||
PIOS_LED_Toggle(LED2);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void eventCallback(UAVObjEvent* ev)
|
||||
{
|
||||
const char* name = UAVObjGetName(ev->obj);
|
||||
}
|
||||
|
||||
static void eventCallbackPeriodic(UAVObjEvent* ev)
|
||||
{
|
||||
static int lastUpdate;
|
||||
testDelay = xTaskGetTickCount() - lastUpdate;
|
||||
lastUpdate = xTaskGetTickCount();
|
||||
const char* name = UAVObjGetName(ev->obj);
|
||||
PIOS_LED_Toggle(LED1);
|
||||
//ExampleObject1Updated();
|
||||
}
|
||||
|
||||
void vApplicationIdleHook(void)
|
||||
{
|
||||
/* Called when the scheduler has no tasks to run */
|
||||
}
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatorcommand.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorCommand object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatorcommand.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "actuatorcommand.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ActuatorCommandInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ACTUATORCOMMAND_OBJID, ACTUATORCOMMAND_NAME, ACTUATORCOMMAND_METANAME, 0,
|
||||
ACTUATORCOMMAND_ISSINGLEINST, ACTUATORCOMMAND_ISSETTINGS, ACTUATORCOMMAND_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ActuatorCommandData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ActuatorCommandData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ActuatorCommandHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatordesired.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorDesired object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatordesired.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "actuatordesired.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ActuatorDesiredInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ACTUATORDESIRED_OBJID, ACTUATORDESIRED_NAME, ACTUATORDESIRED_METANAME, 0,
|
||||
ACTUATORDESIRED_ISSINGLEINST, ACTUATORDESIRED_ISSETTINGS, ACTUATORDESIRED_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ActuatorDesiredData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ActuatorDesiredData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ActuatorDesiredHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,142 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatorsettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatorsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "actuatorsettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ActuatorSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ACTUATORSETTINGS_OBJID, ACTUATORSETTINGS_NAME, ACTUATORSETTINGS_METANAME, 0,
|
||||
ACTUATORSETTINGS_ISSINGLEINST, ACTUATORSETTINGS_ISSETTINGS, ACTUATORSETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ActuatorSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ActuatorSettingsData));
|
||||
data.FixedWingRoll1 = 8;
|
||||
data.FixedWingRoll2 = 8;
|
||||
data.FixedWingPitch1 = 8;
|
||||
data.FixedWingPitch2 = 8;
|
||||
data.FixedWingYaw = 8;
|
||||
data.FixedWingThrottle = 8;
|
||||
data.VTOLMotorN = 8;
|
||||
data.VTOLMotorNE = 8;
|
||||
data.VTOLMotorE = 8;
|
||||
data.VTOLMotorSE = 8;
|
||||
data.VTOLMotorS = 8;
|
||||
data.VTOLMotorSW = 8;
|
||||
data.VTOLMotorW = 8;
|
||||
data.VTOLMotorNW = 8;
|
||||
data.ChannelUpdateFreq[0] = 50;
|
||||
data.ChannelUpdateFreq[1] = 50;
|
||||
data.ChannelMax[0] = 2000;
|
||||
data.ChannelMax[1] = 2000;
|
||||
data.ChannelMax[2] = 2000;
|
||||
data.ChannelMax[3] = 2000;
|
||||
data.ChannelMax[4] = 2000;
|
||||
data.ChannelMax[5] = 2000;
|
||||
data.ChannelMax[6] = 2000;
|
||||
data.ChannelMax[7] = 2000;
|
||||
data.ChannelNeutral[0] = 1500;
|
||||
data.ChannelNeutral[1] = 1500;
|
||||
data.ChannelNeutral[2] = 1500;
|
||||
data.ChannelNeutral[3] = 1500;
|
||||
data.ChannelNeutral[4] = 1500;
|
||||
data.ChannelNeutral[5] = 1500;
|
||||
data.ChannelNeutral[6] = 1500;
|
||||
data.ChannelNeutral[7] = 1500;
|
||||
data.ChannelMin[0] = 1000;
|
||||
data.ChannelMin[1] = 1000;
|
||||
data.ChannelMin[2] = 1000;
|
||||
data.ChannelMin[3] = 1000;
|
||||
data.ChannelMin[4] = 1000;
|
||||
data.ChannelMin[5] = 1000;
|
||||
data.ChannelMin[6] = 1000;
|
||||
data.ChannelMin[7] = 1000;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ActuatorSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrsstatus.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AhrsStatus object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: ahrsstatus.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "ahrsstatus.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t AhrsStatusInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(AHRSSTATUS_OBJID, AHRSSTATUS_NAME, AHRSSTATUS_METANAME, 0,
|
||||
AHRSSTATUS_ISSINGLEINST, AHRSSTATUS_ISSETTINGS, AHRSSTATUS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
AhrsStatusData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AhrsStatusData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.loggingUpdatePeriod = 1000;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle AhrsStatusHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file altitudeactual.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AltitudeActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: altitudeactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "altitudeactual.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t AltitudeActualInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ALTITUDEACTUAL_OBJID, ALTITUDEACTUAL_NAME, ALTITUDEACTUAL_METANAME, 0,
|
||||
ALTITUDEACTUAL_ISSINGLEINST, ALTITUDEACTUAL_ISSETTINGS, ALTITUDEACTUAL_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
AltitudeActualData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AltitudeActualData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle AltitudeActualHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudeactual.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudeactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "attitudeactual.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t AttitudeActualInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ATTITUDEACTUAL_OBJID, ATTITUDEACTUAL_NAME, ATTITUDEACTUAL_METANAME, 0,
|
||||
ATTITUDEACTUAL_ISSINGLEINST, ATTITUDEACTUAL_ISSETTINGS, ATTITUDEACTUAL_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
AttitudeActualData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AttitudeActualData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 500;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle AttitudeActualHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudedesired.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeDesired object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudedesired.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "attitudedesired.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t AttitudeDesiredInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ATTITUDEDESIRED_OBJID, ATTITUDEDESIRED_NAME, ATTITUDEDESIRED_METANAME, 0,
|
||||
ATTITUDEDESIRED_ISSINGLEINST, ATTITUDEDESIRED_ISSETTINGS, ATTITUDEDESIRED_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
AttitudeDesiredData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AttitudeDesiredData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle AttitudeDesiredHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,103 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudesettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudesettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "attitudesettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t AttitudeSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(ATTITUDESETTINGS_OBJID, ATTITUDESETTINGS_NAME, ATTITUDESETTINGS_METANAME, 0,
|
||||
ATTITUDESETTINGS_ISSINGLEINST, ATTITUDESETTINGS_ISSETTINGS, ATTITUDESETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
AttitudeSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AttitudeSettingsData));
|
||||
data.UpdatePeriod = 500;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle AttitudeSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,86 +0,0 @@
|
||||
##
|
||||
##############################################################################
|
||||
#
|
||||
# @file attitudesettings.py
|
||||
# @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
# @brief Implementation of the AttitudeSettings object. This file has been
|
||||
# automatically generated by the UAVObjectGenerator.
|
||||
#
|
||||
# @note Object definition file: attitudesettings.xml.
|
||||
# This is an automatically generated file.
|
||||
# DO NOT modify manually.
|
||||
#
|
||||
# @see The GNU Public License (GPL) Version 3
|
||||
#
|
||||
#############################################################################/
|
||||
#
|
||||
# This program is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# This program is distributed in the hope that it will be useful, but
|
||||
# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
# for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License along
|
||||
# with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
#
|
||||
|
||||
|
||||
import uavobject
|
||||
|
||||
import struct
|
||||
from collections import namedtuple
|
||||
|
||||
# This is a list of instances of the data fields contained in this object
|
||||
_fields = [ \
|
||||
uavobject.UAVObjectField(
|
||||
'UpdatePeriod',
|
||||
'i',
|
||||
1,
|
||||
[
|
||||
'0',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
class AttitudeSettings(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 3446368842
|
||||
NAME = "AttitudeSettings"
|
||||
METANAME = "AttitudeSettingsMeta"
|
||||
ISSINGLEINST = 1
|
||||
ISSETTINGS = 1
|
||||
|
||||
def __init__(self):
|
||||
uavobject.UAVObject.__init__(self,
|
||||
self.OBJID,
|
||||
self.NAME,
|
||||
self.METANAME,
|
||||
0,
|
||||
self.ISSINGLEINST)
|
||||
|
||||
for f in _fields:
|
||||
self.add_field(f)
|
||||
|
||||
def __str__(self):
|
||||
s = ("0x%08X (%10u) %-30s %3u bytes format '%s'\n"
|
||||
% (self.OBJID, self.OBJID, self.NAME, self.get_struct().size, self.get_struct().format))
|
||||
for f in self.get_tuple()._fields:
|
||||
s += ("\t%s\n" % f)
|
||||
return (s)
|
||||
|
||||
def main():
|
||||
# Instantiate the object and dump out some interesting info
|
||||
x = AttitudeSettings()
|
||||
print (x)
|
||||
|
||||
if __name__ == "__main__":
|
||||
#import pdb ; pdb.run('main()')
|
||||
main()
|
@ -1,375 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file eventdispatcher.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Event dispatcher, distributes object events as callbacks. Alternative
|
||||
* to using tasks and queues. All callbacks are invoked from the event task.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 20
|
||||
#define STACK_SIZE configMINIMAL_STACK_SIZE
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define MAX_UPDATE_PERIOD_MS 1000
|
||||
|
||||
// Private types
|
||||
|
||||
|
||||
/**
|
||||
* Event callback information
|
||||
*/
|
||||
typedef struct {
|
||||
UAVObjEvent ev; /** The actual event */
|
||||
UAVObjEventCallback cb; /** The callback function, or zero if none */
|
||||
xQueueHandle queue; /** The queue or zero if none */
|
||||
} EventCallbackInfo;
|
||||
|
||||
/**
|
||||
* List of object properties that are needed for the periodic updates.
|
||||
*/
|
||||
struct PeriodicObjectListStruct {
|
||||
EventCallbackInfo evInfo; /** Event callback information */
|
||||
int32_t updatePeriodMs; /** Update period in ms or 0 if no periodic updates are needed */
|
||||
int32_t timeToNextUpdateMs; /** Time delay to the next update */
|
||||
struct PeriodicObjectListStruct* next; /** Needed by linked list library (utlist.h) */
|
||||
};
|
||||
typedef struct PeriodicObjectListStruct PeriodicObjectList;
|
||||
|
||||
// Private variables
|
||||
static PeriodicObjectList* objList;
|
||||
static xQueueHandle queue;
|
||||
static xTaskHandle eventTaskHandle;
|
||||
static xSemaphoreHandle mutex;
|
||||
static EventStats stats;
|
||||
|
||||
// Private functions
|
||||
static int32_t processPeriodicUpdates();
|
||||
static void eventTask();
|
||||
static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs);
|
||||
static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs);
|
||||
static uint32_t randomizePeriod(uint32_t periodMs);
|
||||
|
||||
|
||||
/**
|
||||
* Initialize the dispatcher
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventDispatcherInitialize()
|
||||
{
|
||||
// Initialize variables
|
||||
objList = NULL;
|
||||
memset(&stats, 0, sizeof(EventStats));
|
||||
|
||||
// Create mutex
|
||||
mutex = xSemaphoreCreateRecursiveMutex();
|
||||
if (mutex == NULL)
|
||||
return -1;
|
||||
|
||||
// Create event queue
|
||||
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
|
||||
|
||||
// Create task
|
||||
xTaskCreate( eventTask, (signed char*)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &eventTaskHandle );
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the statistics counters
|
||||
* @param[out] statsOut The statistics counters will be copied there
|
||||
*/
|
||||
void EventGetStats(EventStats* statsOut)
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
memcpy(statsOut, &stats, sizeof(EventStats));
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear the statistics counters
|
||||
*/
|
||||
void EventClearStats()
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
memset(&stats, 0, sizeof(EventStats));
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Dispatch an event by invoking the supplied callback. The function
|
||||
* returns imidiatelly, the callback is invoked from the event task.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] cb The callback function
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb)
|
||||
{
|
||||
EventCallbackInfo evInfo;
|
||||
// Initialize event callback information
|
||||
memcpy(&evInfo.ev, ev, sizeof(UAVObjEvent));
|
||||
evInfo.cb = cb;
|
||||
evInfo.queue = 0;
|
||||
// Push to queue
|
||||
return xQueueSend(queue, &evInfo, 0); // will not block if queue is full
|
||||
}
|
||||
|
||||
/**
|
||||
* Dispatch an event at periodic intervals.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] cb The callback to be invoked
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
|
||||
{
|
||||
return eventPeriodicCreate(ev, cb, 0, periodMs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the period of a periodic event.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] cb The callback to be invoked
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs)
|
||||
{
|
||||
return eventPeriodicUpdate(ev, cb, 0, periodMs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Dispatch an event at periodic intervals.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] queue The queue that the event will be pushed in
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs)
|
||||
{
|
||||
return eventPeriodicCreate(ev, 0, queue, periodMs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the period of a periodic event.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] queue The queue
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs)
|
||||
{
|
||||
return eventPeriodicUpdate(ev, 0, queue, periodMs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Dispatch an event through a callback at periodic intervals.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] cb The callback to be invoked or zero if none
|
||||
* \param[in] queue The queue or zero if none
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs)
|
||||
{
|
||||
PeriodicObjectList* objEntry;
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
// Check that the object is not already connected
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
if (objEntry->evInfo.cb == cb &&
|
||||
objEntry->evInfo.queue == queue &&
|
||||
objEntry->evInfo.ev.obj == ev->obj &&
|
||||
objEntry->evInfo.ev.instId == ev->instId &&
|
||||
objEntry->evInfo.ev.event == ev->event)
|
||||
{
|
||||
// Already registered, do nothing
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
// Create handle
|
||||
objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList));
|
||||
if (objEntry == NULL) return -1;
|
||||
objEntry->evInfo.ev.obj = ev->obj;
|
||||
objEntry->evInfo.ev.instId = ev->instId;
|
||||
objEntry->evInfo.ev.event = ev->event;
|
||||
objEntry->evInfo.cb = cb;
|
||||
objEntry->evInfo.queue = queue;
|
||||
objEntry->updatePeriodMs = periodMs;
|
||||
objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
|
||||
// Add to list
|
||||
LL_APPEND(objList, objEntry);
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Update the period of a periodic event.
|
||||
* \param[in] ev The event to be dispatched
|
||||
* \param[in] cb The callback to be invoked or zero if none
|
||||
* \param[in] queue The queue or zero if none
|
||||
* \param[in] periodMs The period the event is generated
|
||||
* \return Success (0), failure (-1)
|
||||
*/
|
||||
static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQueueHandle queue, int32_t periodMs)
|
||||
{
|
||||
PeriodicObjectList* objEntry;
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
// Find object
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
if (objEntry->evInfo.cb == cb &&
|
||||
objEntry->evInfo.queue == queue &&
|
||||
objEntry->evInfo.ev.obj == ev->obj &&
|
||||
objEntry->evInfo.ev.instId == ev->instId &&
|
||||
objEntry->evInfo.ev.event == ev->event)
|
||||
{
|
||||
// Object found, update period
|
||||
objEntry->updatePeriodMs = periodMs;
|
||||
objEntry->timeToNextUpdateMs = randomizePeriod(periodMs); // avoid bunching of updates
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
// If this point is reached the object was not found
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Event task, responsible of invoking callbacks.
|
||||
*/
|
||||
static void eventTask()
|
||||
{
|
||||
int32_t timeToNextUpdateMs;
|
||||
int32_t delayMs;
|
||||
EventCallbackInfo evInfo;
|
||||
|
||||
// Initialize time
|
||||
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
|
||||
|
||||
// Loop forever
|
||||
while (1)
|
||||
{
|
||||
// Calculate delay time
|
||||
delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS);
|
||||
if (delayMs < 0)
|
||||
{
|
||||
delayMs = 0;
|
||||
}
|
||||
|
||||
// Wait for queue message
|
||||
if ( xQueueReceive(queue, &evInfo, delayMs/portTICK_RATE_MS) == pdTRUE )
|
||||
{
|
||||
// Invoke callback, if one
|
||||
if ( evInfo.cb != 0)
|
||||
{
|
||||
evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
|
||||
}
|
||||
}
|
||||
|
||||
// Process periodic updates
|
||||
if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
|
||||
{
|
||||
timeToNextUpdateMs = processPeriodicUpdates();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle periodic updates for all objects.
|
||||
* \return The system time until the next update (in ms) or -1 if failed
|
||||
*/
|
||||
static int32_t processPeriodicUpdates()
|
||||
{
|
||||
PeriodicObjectList* objEntry;
|
||||
int32_t timeNow;
|
||||
int32_t timeToNextUpdate;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Iterate through each object and update its timer, if zero then transmit object.
|
||||
// Also calculate smallest delay to next update.
|
||||
timeToNextUpdate = xTaskGetTickCount()*portTICK_RATE_MS + MAX_UPDATE_PERIOD_MS;
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// If object is configured for periodic updates
|
||||
if (objEntry->updatePeriodMs > 0)
|
||||
{
|
||||
// Check if time for the next update
|
||||
timeNow = xTaskGetTickCount()*portTICK_RATE_MS;
|
||||
if (objEntry->timeToNextUpdateMs <= timeNow)
|
||||
{
|
||||
// Reset timer
|
||||
objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs;
|
||||
// Invoke callback, if one
|
||||
if ( objEntry->evInfo.cb != 0)
|
||||
{
|
||||
objEntry->evInfo.cb(&objEntry->evInfo.ev); // the function is expected to copy the event information
|
||||
}
|
||||
// Push event to queue, if one
|
||||
if ( objEntry->evInfo.queue != 0)
|
||||
{
|
||||
if ( xQueueSend(objEntry->evInfo.queue, &objEntry->evInfo.ev, 0) != pdTRUE ) // do not block if queue is full
|
||||
{
|
||||
++stats.eventErrors;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Update minimum delay
|
||||
if (objEntry->timeToNextUpdateMs < timeToNextUpdate)
|
||||
{
|
||||
timeToNextUpdate = objEntry->timeToNextUpdateMs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return timeToNextUpdate;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a psedorandom integer from 0 to periodMs
|
||||
* Based on the Park-Miller-Carta Pseudo-Random Number Generator
|
||||
* http://www.firstpr.com.au/dsp/rand31/
|
||||
*/
|
||||
static uint32_t randomizePeriod(uint32_t periodMs)
|
||||
{
|
||||
static uint32_t seed = 1;
|
||||
uint32_t hi, lo;
|
||||
lo = 16807 * (seed & 0xFFFF);
|
||||
hi = 16807 * (seed >> 16);
|
||||
lo += (hi & 0x7FFF) << 16;
|
||||
lo += hi >> 15;
|
||||
if (lo > 0x7FFFFFFF) lo -= 0x7FFFFFFF;
|
||||
seed = lo;
|
||||
return (uint32_t)( ((float)periodMs * (float)lo) / (float)0x7FFFFFFF );
|
||||
}
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file exampleobject1.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleObject1 object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: exampleobject1.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "exampleobject1.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ExampleObject1Initialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(EXAMPLEOBJECT1_OBJID, EXAMPLEOBJECT1_NAME, EXAMPLEOBJECT1_METANAME, 0,
|
||||
EXAMPLEOBJECT1_ISSINGLEINST, EXAMPLEOBJECT1_ISSETTINGS, EXAMPLEOBJECT1_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ExampleObject1Data data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ExampleObject1Data));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.gcsTelemetryUpdatePeriod = 500;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ExampleObject1Handle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file exampleobject2.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleObject2 object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: exampleobject2.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "exampleobject2.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ExampleObject2Initialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(EXAMPLEOBJECT2_OBJID, EXAMPLEOBJECT2_NAME, EXAMPLEOBJECT2_METANAME, 0,
|
||||
EXAMPLEOBJECT2_ISSINGLEINST, EXAMPLEOBJECT2_ISSETTINGS, EXAMPLEOBJECT2_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ExampleObject2Data data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ExampleObject2Data));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 500;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ExampleObject2Handle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,105 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplesettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: examplesettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "examplesettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ExampleSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(EXAMPLESETTINGS_OBJID, EXAMPLESETTINGS_NAME, EXAMPLESETTINGS_METANAME, 0,
|
||||
EXAMPLESETTINGS_ISSINGLEINST, EXAMPLESETTINGS_ISSETTINGS, EXAMPLESETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ExampleSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ExampleSettingsData));
|
||||
data.UpdatePeriod = 10;
|
||||
data.StepSize = 1;
|
||||
data.StepDirection = 0;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ExampleSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file flightbatterystate.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the FlightBatteryState object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: flightbatterystate.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "flightbatterystate.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t FlightBatteryStateInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(FLIGHTBATTERYSTATE_OBJID, FLIGHTBATTERYSTATE_NAME, FLIGHTBATTERYSTATE_METANAME, 0,
|
||||
FLIGHTBATTERYSTATE_ISSINGLEINST, FLIGHTBATTERYSTATE_ISSETTINGS, FLIGHTBATTERYSTATE_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
FlightBatteryStateData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(FlightBatteryStateData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READONLY;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle FlightBatteryStateHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file flighttelemetrystats.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the FlightTelemetryStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: flighttelemetrystats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "flighttelemetrystats.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t FlightTelemetryStatsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(FLIGHTTELEMETRYSTATS_OBJID, FLIGHTTELEMETRYSTATS_NAME, FLIGHTTELEMETRYSTATS_METANAME, 0,
|
||||
FLIGHTTELEMETRYSTATS_ISSINGLEINST, FLIGHTTELEMETRYSTATS_ISSETTINGS, FLIGHTTELEMETRYSTATS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
FlightTelemetryStatsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(FlightTelemetryStatsData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 5000;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.loggingUpdatePeriod = 5000;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle FlightTelemetryStatsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file gcstelemetrystats.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the GCSTelemetryStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: gcstelemetrystats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "gcstelemetrystats.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t GCSTelemetryStatsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(GCSTELEMETRYSTATS_OBJID, GCSTELEMETRYSTATS_NAME, GCSTELEMETRYSTATS_METANAME, 0,
|
||||
GCSTELEMETRYSTATS_ISSINGLEINST, GCSTELEMETRYSTATS_ISSETTINGS, GCSTELEMETRYSTATS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
GCSTelemetryStatsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(GCSTelemetryStatsData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.gcsTelemetryUpdatePeriod = 5000;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle GCSTelemetryStatsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file headingactual.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the HeadingActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: headingactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "headingactual.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t HeadingActualInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(HEADINGACTUAL_OBJID, HEADINGACTUAL_NAME, HEADINGACTUAL_METANAME, 0,
|
||||
HEADINGACTUAL_ISSINGLEINST, HEADINGACTUAL_ISSETTINGS, HEADINGACTUAL_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
HeadingActualData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(HeadingActualData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 500;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle HeadingActualHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,74 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatorcommand.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorCommand object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatorcommand.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ACTUATORCOMMAND_H
|
||||
#define ACTUATORCOMMAND_H
|
||||
|
||||
// Object constants
|
||||
#define ACTUATORCOMMAND_OBJID 3909877022U
|
||||
#define ACTUATORCOMMAND_NAME "ActuatorCommand"
|
||||
#define ACTUATORCOMMAND_METANAME "ActuatorCommandMeta"
|
||||
#define ACTUATORCOMMAND_ISSINGLEINST 1
|
||||
#define ACTUATORCOMMAND_ISSETTINGS 0
|
||||
#define ACTUATORCOMMAND_NUMBYTES sizeof(ActuatorCommandData)
|
||||
|
||||
// Object access macros
|
||||
#define ActuatorCommandGet(dataOut) UAVObjGetData(ActuatorCommandHandle(), dataOut)
|
||||
#define ActuatorCommandSet(dataIn) UAVObjSetData(ActuatorCommandHandle(), dataIn)
|
||||
#define ActuatorCommandInstGet(instId, dataOut) UAVObjGetInstanceData(ActuatorCommandHandle(), instId, dataOut)
|
||||
#define ActuatorCommandInstSet(instId, dataIn) UAVObjSetInstanceData(ActuatorCommandHandle(), instId, dataIn)
|
||||
#define ActuatorCommandConnectQueue(queue) UAVObjConnectQueue(ActuatorCommandHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorCommandConnectCallback(cb) UAVObjConnectCallback(ActuatorCommandHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorCommandCreateInstance() UAVObjCreateInstance(ActuatorCommandHandle())
|
||||
#define ActuatorCommandRequestUpdate() UAVObjRequestUpdate(ActuatorCommandHandle())
|
||||
#define ActuatorCommandRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ActuatorCommandHandle(), instId)
|
||||
#define ActuatorCommandUpdated() UAVObjUpdated(ActuatorCommandHandle())
|
||||
#define ActuatorCommandInstUpdated(instId) UAVObjUpdated(ActuatorCommandHandle(), instId)
|
||||
#define ActuatorCommandGetMetadata(dataOut) UAVObjGetMetadata(ActuatorCommandHandle(), dataOut)
|
||||
#define ActuatorCommandSetMetadata(dataIn) UAVObjSetMetadata(ActuatorCommandHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int16_t Channel[8];
|
||||
|
||||
} __attribute__((packed)) ActuatorCommandData;
|
||||
|
||||
// Field information
|
||||
// Field Channel information
|
||||
/* Number of elements for field Channel */
|
||||
#define ACTUATORCOMMAND_CHANNEL_NUMELEM 8
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ActuatorCommandInitialize();
|
||||
UAVObjHandle ActuatorCommandHandle();
|
||||
|
||||
#endif // ACTUATORCOMMAND_H
|
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatordesired.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorDesired object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatordesired.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ACTUATORDESIRED_H
|
||||
#define ACTUATORDESIRED_H
|
||||
|
||||
// Object constants
|
||||
#define ACTUATORDESIRED_OBJID 123085850U
|
||||
#define ACTUATORDESIRED_NAME "ActuatorDesired"
|
||||
#define ACTUATORDESIRED_METANAME "ActuatorDesiredMeta"
|
||||
#define ACTUATORDESIRED_ISSINGLEINST 1
|
||||
#define ACTUATORDESIRED_ISSETTINGS 0
|
||||
#define ACTUATORDESIRED_NUMBYTES sizeof(ActuatorDesiredData)
|
||||
|
||||
// Object access macros
|
||||
#define ActuatorDesiredGet(dataOut) UAVObjGetData(ActuatorDesiredHandle(), dataOut)
|
||||
#define ActuatorDesiredSet(dataIn) UAVObjSetData(ActuatorDesiredHandle(), dataIn)
|
||||
#define ActuatorDesiredInstGet(instId, dataOut) UAVObjGetInstanceData(ActuatorDesiredHandle(), instId, dataOut)
|
||||
#define ActuatorDesiredInstSet(instId, dataIn) UAVObjSetInstanceData(ActuatorDesiredHandle(), instId, dataIn)
|
||||
#define ActuatorDesiredConnectQueue(queue) UAVObjConnectQueue(ActuatorDesiredHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorDesiredConnectCallback(cb) UAVObjConnectCallback(ActuatorDesiredHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorDesiredCreateInstance() UAVObjCreateInstance(ActuatorDesiredHandle())
|
||||
#define ActuatorDesiredRequestUpdate() UAVObjRequestUpdate(ActuatorDesiredHandle())
|
||||
#define ActuatorDesiredRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ActuatorDesiredHandle(), instId)
|
||||
#define ActuatorDesiredUpdated() UAVObjUpdated(ActuatorDesiredHandle())
|
||||
#define ActuatorDesiredInstUpdated(instId) UAVObjUpdated(ActuatorDesiredHandle(), instId)
|
||||
#define ActuatorDesiredGetMetadata(dataOut) UAVObjGetMetadata(ActuatorDesiredHandle(), dataOut)
|
||||
#define ActuatorDesiredSetMetadata(dataIn) UAVObjSetMetadata(ActuatorDesiredHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
float Throttle;
|
||||
|
||||
} __attribute__((packed)) ActuatorDesiredData;
|
||||
|
||||
// Field information
|
||||
// Field Roll information
|
||||
// Field Pitch information
|
||||
// Field Yaw information
|
||||
// Field Throttle information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ActuatorDesiredInitialize();
|
||||
UAVObjHandle ActuatorDesiredHandle();
|
||||
|
||||
#endif // ACTUATORDESIRED_H
|
@ -1,142 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file actuatorsettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ActuatorSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: actuatorsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ACTUATORSETTINGS_H
|
||||
#define ACTUATORSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define ACTUATORSETTINGS_OBJID 3054509114U
|
||||
#define ACTUATORSETTINGS_NAME "ActuatorSettings"
|
||||
#define ACTUATORSETTINGS_METANAME "ActuatorSettingsMeta"
|
||||
#define ACTUATORSETTINGS_ISSINGLEINST 1
|
||||
#define ACTUATORSETTINGS_ISSETTINGS 1
|
||||
#define ACTUATORSETTINGS_NUMBYTES sizeof(ActuatorSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define ActuatorSettingsGet(dataOut) UAVObjGetData(ActuatorSettingsHandle(), dataOut)
|
||||
#define ActuatorSettingsSet(dataIn) UAVObjSetData(ActuatorSettingsHandle(), dataIn)
|
||||
#define ActuatorSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(ActuatorSettingsHandle(), instId, dataOut)
|
||||
#define ActuatorSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(ActuatorSettingsHandle(), instId, dataIn)
|
||||
#define ActuatorSettingsConnectQueue(queue) UAVObjConnectQueue(ActuatorSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorSettingsConnectCallback(cb) UAVObjConnectCallback(ActuatorSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ActuatorSettingsCreateInstance() UAVObjCreateInstance(ActuatorSettingsHandle())
|
||||
#define ActuatorSettingsRequestUpdate() UAVObjRequestUpdate(ActuatorSettingsHandle())
|
||||
#define ActuatorSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ActuatorSettingsHandle(), instId)
|
||||
#define ActuatorSettingsUpdated() UAVObjUpdated(ActuatorSettingsHandle())
|
||||
#define ActuatorSettingsInstUpdated(instId) UAVObjUpdated(ActuatorSettingsHandle(), instId)
|
||||
#define ActuatorSettingsGetMetadata(dataOut) UAVObjGetMetadata(ActuatorSettingsHandle(), dataOut)
|
||||
#define ActuatorSettingsSetMetadata(dataIn) UAVObjSetMetadata(ActuatorSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t FixedWingRoll1;
|
||||
uint8_t FixedWingRoll2;
|
||||
uint8_t FixedWingPitch1;
|
||||
uint8_t FixedWingPitch2;
|
||||
uint8_t FixedWingYaw;
|
||||
uint8_t FixedWingThrottle;
|
||||
uint8_t VTOLMotorN;
|
||||
uint8_t VTOLMotorNE;
|
||||
uint8_t VTOLMotorE;
|
||||
uint8_t VTOLMotorSE;
|
||||
uint8_t VTOLMotorS;
|
||||
uint8_t VTOLMotorSW;
|
||||
uint8_t VTOLMotorW;
|
||||
uint8_t VTOLMotorNW;
|
||||
int16_t ChannelUpdateFreq[2];
|
||||
int16_t ChannelMax[8];
|
||||
int16_t ChannelNeutral[8];
|
||||
int16_t ChannelMin[8];
|
||||
|
||||
} __attribute__((packed)) ActuatorSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field FixedWingRoll1 information
|
||||
/* Enumeration options for field FixedWingRoll1 */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGROLL1_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGROLL1_NONE=8, } ActuatorSettingsFixedWingRoll1Options;
|
||||
// Field FixedWingRoll2 information
|
||||
/* Enumeration options for field FixedWingRoll2 */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGROLL2_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGROLL2_NONE=8, } ActuatorSettingsFixedWingRoll2Options;
|
||||
// Field FixedWingPitch1 information
|
||||
/* Enumeration options for field FixedWingPitch1 */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGPITCH1_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGPITCH1_NONE=8, } ActuatorSettingsFixedWingPitch1Options;
|
||||
// Field FixedWingPitch2 information
|
||||
/* Enumeration options for field FixedWingPitch2 */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGPITCH2_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGPITCH2_NONE=8, } ActuatorSettingsFixedWingPitch2Options;
|
||||
// Field FixedWingYaw information
|
||||
/* Enumeration options for field FixedWingYaw */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGYAW_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGYAW_NONE=8, } ActuatorSettingsFixedWingYawOptions;
|
||||
// Field FixedWingThrottle information
|
||||
/* Enumeration options for field FixedWingThrottle */
|
||||
typedef enum { ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL0=0, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL1=1, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL2=2, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL3=3, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL4=4, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL5=5, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL6=6, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_CHANNEL7=7, ACTUATORSETTINGS_FIXEDWINGTHROTTLE_NONE=8, } ActuatorSettingsFixedWingThrottleOptions;
|
||||
// Field VTOLMotorN information
|
||||
/* Enumeration options for field VTOLMotorN */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORN_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORN_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORN_NONE=8, } ActuatorSettingsVTOLMotorNOptions;
|
||||
// Field VTOLMotorNE information
|
||||
/* Enumeration options for field VTOLMotorNE */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORNE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORNE_NONE=8, } ActuatorSettingsVTOLMotorNEOptions;
|
||||
// Field VTOLMotorE information
|
||||
/* Enumeration options for field VTOLMotorE */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORE_NONE=8, } ActuatorSettingsVTOLMotorEOptions;
|
||||
// Field VTOLMotorSE information
|
||||
/* Enumeration options for field VTOLMotorSE */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORSE_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORSE_NONE=8, } ActuatorSettingsVTOLMotorSEOptions;
|
||||
// Field VTOLMotorS information
|
||||
/* Enumeration options for field VTOLMotorS */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORS_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORS_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORS_NONE=8, } ActuatorSettingsVTOLMotorSOptions;
|
||||
// Field VTOLMotorSW information
|
||||
/* Enumeration options for field VTOLMotorSW */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORSW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORSW_NONE=8, } ActuatorSettingsVTOLMotorSWOptions;
|
||||
// Field VTOLMotorW information
|
||||
/* Enumeration options for field VTOLMotorW */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORW_NONE=8, } ActuatorSettingsVTOLMotorWOptions;
|
||||
// Field VTOLMotorNW information
|
||||
/* Enumeration options for field VTOLMotorNW */
|
||||
typedef enum { ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL0=0, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL1=1, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL2=2, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL3=3, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL4=4, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL5=5, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL6=6, ACTUATORSETTINGS_VTOLMOTORNW_CHANNEL7=7, ACTUATORSETTINGS_VTOLMOTORNW_NONE=8, } ActuatorSettingsVTOLMotorNWOptions;
|
||||
// Field ChannelUpdateFreq information
|
||||
/* Number of elements for field ChannelUpdateFreq */
|
||||
#define ACTUATORSETTINGS_CHANNELUPDATEFREQ_NUMELEM 2
|
||||
// Field ChannelMax information
|
||||
/* Number of elements for field ChannelMax */
|
||||
#define ACTUATORSETTINGS_CHANNELMAX_NUMELEM 8
|
||||
// Field ChannelNeutral information
|
||||
/* Number of elements for field ChannelNeutral */
|
||||
#define ACTUATORSETTINGS_CHANNELNEUTRAL_NUMELEM 8
|
||||
// Field ChannelMin information
|
||||
/* Number of elements for field ChannelMin */
|
||||
#define ACTUATORSETTINGS_CHANNELMIN_NUMELEM 8
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ActuatorSettingsInitialize();
|
||||
UAVObjHandle ActuatorSettingsHandle();
|
||||
|
||||
#endif // ACTUATORSETTINGS_H
|
@ -1,74 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file ahrsstatus.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AhrsStatus object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: ahrsstatus.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef AHRSSTATUS_H
|
||||
#define AHRSSTATUS_H
|
||||
|
||||
// Object constants
|
||||
#define AHRSSTATUS_OBJID 3344048156U
|
||||
#define AHRSSTATUS_NAME "AhrsStatus"
|
||||
#define AHRSSTATUS_METANAME "AhrsStatusMeta"
|
||||
#define AHRSSTATUS_ISSINGLEINST 1
|
||||
#define AHRSSTATUS_ISSETTINGS 0
|
||||
#define AHRSSTATUS_NUMBYTES sizeof(AhrsStatusData)
|
||||
|
||||
// Object access macros
|
||||
#define AhrsStatusGet(dataOut) UAVObjGetData(AhrsStatusHandle(), dataOut)
|
||||
#define AhrsStatusSet(dataIn) UAVObjSetData(AhrsStatusHandle(), dataIn)
|
||||
#define AhrsStatusInstGet(instId, dataOut) UAVObjGetInstanceData(AhrsStatusHandle(), instId, dataOut)
|
||||
#define AhrsStatusInstSet(instId, dataIn) UAVObjSetInstanceData(AhrsStatusHandle(), instId, dataIn)
|
||||
#define AhrsStatusConnectQueue(queue) UAVObjConnectQueue(AhrsStatusHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define AhrsStatusConnectCallback(cb) UAVObjConnectCallback(AhrsStatusHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define AhrsStatusCreateInstance() UAVObjCreateInstance(AhrsStatusHandle())
|
||||
#define AhrsStatusRequestUpdate() UAVObjRequestUpdate(AhrsStatusHandle())
|
||||
#define AhrsStatusRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AhrsStatusHandle(), instId)
|
||||
#define AhrsStatusUpdated() UAVObjUpdated(AhrsStatusHandle())
|
||||
#define AhrsStatusInstUpdated(instId) UAVObjUpdated(AhrsStatusHandle(), instId)
|
||||
#define AhrsStatusGetMetadata(dataOut) UAVObjGetMetadata(AhrsStatusHandle(), dataOut)
|
||||
#define AhrsStatusSetMetadata(dataIn) UAVObjSetMetadata(AhrsStatusHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t SerialNumber[25];
|
||||
|
||||
} __attribute__((packed)) AhrsStatusData;
|
||||
|
||||
// Field information
|
||||
// Field SerialNumber information
|
||||
/* Number of elements for field SerialNumber */
|
||||
#define AHRSSTATUS_SERIALNUMBER_NUMELEM 25
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t AhrsStatusInitialize();
|
||||
UAVObjHandle AhrsStatusHandle();
|
||||
|
||||
#endif // AHRSSTATUS_H
|
@ -1,76 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file altitudeactual.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AltitudeActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: altitudeactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ALTITUDEACTUAL_H
|
||||
#define ALTITUDEACTUAL_H
|
||||
|
||||
// Object constants
|
||||
#define ALTITUDEACTUAL_OBJID 2251817750U
|
||||
#define ALTITUDEACTUAL_NAME "AltitudeActual"
|
||||
#define ALTITUDEACTUAL_METANAME "AltitudeActualMeta"
|
||||
#define ALTITUDEACTUAL_ISSINGLEINST 1
|
||||
#define ALTITUDEACTUAL_ISSETTINGS 0
|
||||
#define ALTITUDEACTUAL_NUMBYTES sizeof(AltitudeActualData)
|
||||
|
||||
// Object access macros
|
||||
#define AltitudeActualGet(dataOut) UAVObjGetData(AltitudeActualHandle(), dataOut)
|
||||
#define AltitudeActualSet(dataIn) UAVObjSetData(AltitudeActualHandle(), dataIn)
|
||||
#define AltitudeActualInstGet(instId, dataOut) UAVObjGetInstanceData(AltitudeActualHandle(), instId, dataOut)
|
||||
#define AltitudeActualInstSet(instId, dataIn) UAVObjSetInstanceData(AltitudeActualHandle(), instId, dataIn)
|
||||
#define AltitudeActualConnectQueue(queue) UAVObjConnectQueue(AltitudeActualHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define AltitudeActualConnectCallback(cb) UAVObjConnectCallback(AltitudeActualHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define AltitudeActualCreateInstance() UAVObjCreateInstance(AltitudeActualHandle())
|
||||
#define AltitudeActualRequestUpdate() UAVObjRequestUpdate(AltitudeActualHandle())
|
||||
#define AltitudeActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AltitudeActualHandle(), instId)
|
||||
#define AltitudeActualUpdated() UAVObjUpdated(AltitudeActualHandle())
|
||||
#define AltitudeActualInstUpdated(instId) UAVObjUpdated(AltitudeActualHandle(), instId)
|
||||
#define AltitudeActualGetMetadata(dataOut) UAVObjGetMetadata(AltitudeActualHandle(), dataOut)
|
||||
#define AltitudeActualSetMetadata(dataIn) UAVObjSetMetadata(AltitudeActualHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
float Altitude;
|
||||
float Temperature;
|
||||
float Pressure;
|
||||
|
||||
} __attribute__((packed)) AltitudeActualData;
|
||||
|
||||
// Field information
|
||||
// Field Altitude information
|
||||
// Field Temperature information
|
||||
// Field Pressure information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t AltitudeActualInitialize();
|
||||
UAVObjHandle AltitudeActualHandle();
|
||||
|
||||
#endif // ALTITUDEACTUAL_H
|
@ -1,84 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudeactual.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudeactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ATTITUDEACTUAL_H
|
||||
#define ATTITUDEACTUAL_H
|
||||
|
||||
// Object constants
|
||||
#define ATTITUDEACTUAL_OBJID 4233858292U
|
||||
#define ATTITUDEACTUAL_NAME "AttitudeActual"
|
||||
#define ATTITUDEACTUAL_METANAME "AttitudeActualMeta"
|
||||
#define ATTITUDEACTUAL_ISSINGLEINST 1
|
||||
#define ATTITUDEACTUAL_ISSETTINGS 0
|
||||
#define ATTITUDEACTUAL_NUMBYTES sizeof(AttitudeActualData)
|
||||
|
||||
// Object access macros
|
||||
#define AttitudeActualGet(dataOut) UAVObjGetData(AttitudeActualHandle(), dataOut)
|
||||
#define AttitudeActualSet(dataIn) UAVObjSetData(AttitudeActualHandle(), dataIn)
|
||||
#define AttitudeActualInstGet(instId, dataOut) UAVObjGetInstanceData(AttitudeActualHandle(), instId, dataOut)
|
||||
#define AttitudeActualInstSet(instId, dataIn) UAVObjSetInstanceData(AttitudeActualHandle(), instId, dataIn)
|
||||
#define AttitudeActualConnectQueue(queue) UAVObjConnectQueue(AttitudeActualHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeActualConnectCallback(cb) UAVObjConnectCallback(AttitudeActualHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeActualCreateInstance() UAVObjCreateInstance(AttitudeActualHandle())
|
||||
#define AttitudeActualRequestUpdate() UAVObjRequestUpdate(AttitudeActualHandle())
|
||||
#define AttitudeActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AttitudeActualHandle(), instId)
|
||||
#define AttitudeActualUpdated() UAVObjUpdated(AttitudeActualHandle())
|
||||
#define AttitudeActualInstUpdated(instId) UAVObjUpdated(AttitudeActualHandle(), instId)
|
||||
#define AttitudeActualGetMetadata(dataOut) UAVObjGetMetadata(AttitudeActualHandle(), dataOut)
|
||||
#define AttitudeActualSetMetadata(dataIn) UAVObjSetMetadata(AttitudeActualHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
float q1;
|
||||
float q2;
|
||||
float q3;
|
||||
float q4;
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
|
||||
} __attribute__((packed)) AttitudeActualData;
|
||||
|
||||
// Field information
|
||||
// Field q1 information
|
||||
// Field q2 information
|
||||
// Field q3 information
|
||||
// Field q4 information
|
||||
// Field Roll information
|
||||
// Field Pitch information
|
||||
// Field Yaw information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t AttitudeActualInitialize();
|
||||
UAVObjHandle AttitudeActualHandle();
|
||||
|
||||
#endif // ATTITUDEACTUAL_H
|
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudedesired.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeDesired object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudedesired.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ATTITUDEDESIRED_H
|
||||
#define ATTITUDEDESIRED_H
|
||||
|
||||
// Object constants
|
||||
#define ATTITUDEDESIRED_OBJID 1412270808U
|
||||
#define ATTITUDEDESIRED_NAME "AttitudeDesired"
|
||||
#define ATTITUDEDESIRED_METANAME "AttitudeDesiredMeta"
|
||||
#define ATTITUDEDESIRED_ISSINGLEINST 1
|
||||
#define ATTITUDEDESIRED_ISSETTINGS 0
|
||||
#define ATTITUDEDESIRED_NUMBYTES sizeof(AttitudeDesiredData)
|
||||
|
||||
// Object access macros
|
||||
#define AttitudeDesiredGet(dataOut) UAVObjGetData(AttitudeDesiredHandle(), dataOut)
|
||||
#define AttitudeDesiredSet(dataIn) UAVObjSetData(AttitudeDesiredHandle(), dataIn)
|
||||
#define AttitudeDesiredInstGet(instId, dataOut) UAVObjGetInstanceData(AttitudeDesiredHandle(), instId, dataOut)
|
||||
#define AttitudeDesiredInstSet(instId, dataIn) UAVObjSetInstanceData(AttitudeDesiredHandle(), instId, dataIn)
|
||||
#define AttitudeDesiredConnectQueue(queue) UAVObjConnectQueue(AttitudeDesiredHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeDesiredConnectCallback(cb) UAVObjConnectCallback(AttitudeDesiredHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeDesiredCreateInstance() UAVObjCreateInstance(AttitudeDesiredHandle())
|
||||
#define AttitudeDesiredRequestUpdate() UAVObjRequestUpdate(AttitudeDesiredHandle())
|
||||
#define AttitudeDesiredRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AttitudeDesiredHandle(), instId)
|
||||
#define AttitudeDesiredUpdated() UAVObjUpdated(AttitudeDesiredHandle())
|
||||
#define AttitudeDesiredInstUpdated(instId) UAVObjUpdated(AttitudeDesiredHandle(), instId)
|
||||
#define AttitudeDesiredGetMetadata(dataOut) UAVObjGetMetadata(AttitudeDesiredHandle(), dataOut)
|
||||
#define AttitudeDesiredSetMetadata(dataIn) UAVObjSetMetadata(AttitudeDesiredHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
float Throttle;
|
||||
|
||||
} __attribute__((packed)) AttitudeDesiredData;
|
||||
|
||||
// Field information
|
||||
// Field Roll information
|
||||
// Field Pitch information
|
||||
// Field Yaw information
|
||||
// Field Throttle information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t AttitudeDesiredInitialize();
|
||||
UAVObjHandle AttitudeDesiredHandle();
|
||||
|
||||
#endif // ATTITUDEDESIRED_H
|
@ -1,72 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file attitudesettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the AttitudeSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: attitudesettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef ATTITUDESETTINGS_H
|
||||
#define ATTITUDESETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define ATTITUDESETTINGS_OBJID 3446368842U
|
||||
#define ATTITUDESETTINGS_NAME "AttitudeSettings"
|
||||
#define ATTITUDESETTINGS_METANAME "AttitudeSettingsMeta"
|
||||
#define ATTITUDESETTINGS_ISSINGLEINST 1
|
||||
#define ATTITUDESETTINGS_ISSETTINGS 1
|
||||
#define ATTITUDESETTINGS_NUMBYTES sizeof(AttitudeSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define AttitudeSettingsGet(dataOut) UAVObjGetData(AttitudeSettingsHandle(), dataOut)
|
||||
#define AttitudeSettingsSet(dataIn) UAVObjSetData(AttitudeSettingsHandle(), dataIn)
|
||||
#define AttitudeSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(AttitudeSettingsHandle(), instId, dataOut)
|
||||
#define AttitudeSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(AttitudeSettingsHandle(), instId, dataIn)
|
||||
#define AttitudeSettingsConnectQueue(queue) UAVObjConnectQueue(AttitudeSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeSettingsConnectCallback(cb) UAVObjConnectCallback(AttitudeSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define AttitudeSettingsCreateInstance() UAVObjCreateInstance(AttitudeSettingsHandle())
|
||||
#define AttitudeSettingsRequestUpdate() UAVObjRequestUpdate(AttitudeSettingsHandle())
|
||||
#define AttitudeSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(AttitudeSettingsHandle(), instId)
|
||||
#define AttitudeSettingsUpdated() UAVObjUpdated(AttitudeSettingsHandle())
|
||||
#define AttitudeSettingsInstUpdated(instId) UAVObjUpdated(AttitudeSettingsHandle(), instId)
|
||||
#define AttitudeSettingsGetMetadata(dataOut) UAVObjGetMetadata(AttitudeSettingsHandle(), dataOut)
|
||||
#define AttitudeSettingsSetMetadata(dataIn) UAVObjSetMetadata(AttitudeSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int32_t UpdatePeriod;
|
||||
|
||||
} __attribute__((packed)) AttitudeSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field UpdatePeriod information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t AttitudeSettingsInitialize();
|
||||
UAVObjHandle AttitudeSettingsHandle();
|
||||
|
||||
#endif // ATTITUDESETTINGS_H
|
@ -1,47 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file eventdispatcher.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include files of the uavobjectlist library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef EVENTDISPATCHER_H
|
||||
#define EVENTDISPATCHER_H
|
||||
|
||||
// Public types
|
||||
/**
|
||||
* Event dispatcher statistics
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t eventErrors;
|
||||
} EventStats;
|
||||
|
||||
// Public functions
|
||||
int32_t EventDispatcherInitialize();
|
||||
void EventGetStats(EventStats* statsOut);
|
||||
void EventClearStats();
|
||||
int32_t EventCallbackDispatch(UAVObjEvent* ev, UAVObjEventCallback cb);
|
||||
int32_t EventPeriodicCallbackCreate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
|
||||
int32_t EventPeriodicCallbackUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, int32_t periodMs);
|
||||
int32_t EventPeriodicQueueCreate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs);
|
||||
int32_t EventPeriodicQueueUpdate(UAVObjEvent* ev, xQueueHandle queue, int32_t periodMs);
|
||||
|
||||
#endif // EVENTDISPATCHER_H
|
@ -1,90 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file exampleobject1.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleObject1 object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: exampleobject1.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef EXAMPLEOBJECT1_H
|
||||
#define EXAMPLEOBJECT1_H
|
||||
|
||||
// Object constants
|
||||
#define EXAMPLEOBJECT1_OBJID 3852936276U
|
||||
#define EXAMPLEOBJECT1_NAME "ExampleObject1"
|
||||
#define EXAMPLEOBJECT1_METANAME "ExampleObject1Meta"
|
||||
#define EXAMPLEOBJECT1_ISSINGLEINST 0
|
||||
#define EXAMPLEOBJECT1_ISSETTINGS 0
|
||||
#define EXAMPLEOBJECT1_NUMBYTES sizeof(ExampleObject1Data)
|
||||
|
||||
// Object access macros
|
||||
#define ExampleObject1Get(dataOut) UAVObjGetData(ExampleObject1Handle(), dataOut)
|
||||
#define ExampleObject1Set(dataIn) UAVObjSetData(ExampleObject1Handle(), dataIn)
|
||||
#define ExampleObject1InstGet(instId, dataOut) UAVObjGetInstanceData(ExampleObject1Handle(), instId, dataOut)
|
||||
#define ExampleObject1InstSet(instId, dataIn) UAVObjSetInstanceData(ExampleObject1Handle(), instId, dataIn)
|
||||
#define ExampleObject1ConnectQueue(queue) UAVObjConnectQueue(ExampleObject1Handle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleObject1ConnectCallback(cb) UAVObjConnectCallback(ExampleObject1Handle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleObject1CreateInstance() UAVObjCreateInstance(ExampleObject1Handle())
|
||||
#define ExampleObject1RequestUpdate() UAVObjRequestUpdate(ExampleObject1Handle())
|
||||
#define ExampleObject1RequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ExampleObject1Handle(), instId)
|
||||
#define ExampleObject1Updated() UAVObjUpdated(ExampleObject1Handle())
|
||||
#define ExampleObject1InstUpdated(instId) UAVObjUpdated(ExampleObject1Handle(), instId)
|
||||
#define ExampleObject1GetMetadata(dataOut) UAVObjGetMetadata(ExampleObject1Handle(), dataOut)
|
||||
#define ExampleObject1SetMetadata(dataIn) UAVObjSetMetadata(ExampleObject1Handle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int8_t Field1;
|
||||
int16_t Field2;
|
||||
int32_t Field3;
|
||||
float Field4[4];
|
||||
uint8_t Field5;
|
||||
uint16_t Field6;
|
||||
uint32_t Field7;
|
||||
uint8_t Field8;
|
||||
|
||||
} __attribute__((packed)) ExampleObject1Data;
|
||||
|
||||
// Field information
|
||||
// Field Field1 information
|
||||
// Field Field2 information
|
||||
// Field Field3 information
|
||||
// Field Field4 information
|
||||
/* Number of elements for field Field4 */
|
||||
#define EXAMPLEOBJECT1_FIELD4_NUMELEM 4
|
||||
// Field Field5 information
|
||||
// Field Field6 information
|
||||
// Field Field7 information
|
||||
// Field Field8 information
|
||||
/* Enumeration options for field Field8 */
|
||||
typedef enum { EXAMPLEOBJECT1_FIELD8_OPTION1=0, EXAMPLEOBJECT1_FIELD8_OPTION2=1, } ExampleObject1Field8Options;
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ExampleObject1Initialize();
|
||||
UAVObjHandle ExampleObject1Handle();
|
||||
|
||||
#endif // EXAMPLEOBJECT1_H
|
@ -1,80 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file exampleobject2.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleObject2 object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: exampleobject2.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef EXAMPLEOBJECT2_H
|
||||
#define EXAMPLEOBJECT2_H
|
||||
|
||||
// Object constants
|
||||
#define EXAMPLEOBJECT2_OBJID 2743296914U
|
||||
#define EXAMPLEOBJECT2_NAME "ExampleObject2"
|
||||
#define EXAMPLEOBJECT2_METANAME "ExampleObject2Meta"
|
||||
#define EXAMPLEOBJECT2_ISSINGLEINST 0
|
||||
#define EXAMPLEOBJECT2_ISSETTINGS 0
|
||||
#define EXAMPLEOBJECT2_NUMBYTES sizeof(ExampleObject2Data)
|
||||
|
||||
// Object access macros
|
||||
#define ExampleObject2Get(dataOut) UAVObjGetData(ExampleObject2Handle(), dataOut)
|
||||
#define ExampleObject2Set(dataIn) UAVObjSetData(ExampleObject2Handle(), dataIn)
|
||||
#define ExampleObject2InstGet(instId, dataOut) UAVObjGetInstanceData(ExampleObject2Handle(), instId, dataOut)
|
||||
#define ExampleObject2InstSet(instId, dataIn) UAVObjSetInstanceData(ExampleObject2Handle(), instId, dataIn)
|
||||
#define ExampleObject2ConnectQueue(queue) UAVObjConnectQueue(ExampleObject2Handle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleObject2ConnectCallback(cb) UAVObjConnectCallback(ExampleObject2Handle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleObject2CreateInstance() UAVObjCreateInstance(ExampleObject2Handle())
|
||||
#define ExampleObject2RequestUpdate() UAVObjRequestUpdate(ExampleObject2Handle())
|
||||
#define ExampleObject2RequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ExampleObject2Handle(), instId)
|
||||
#define ExampleObject2Updated() UAVObjUpdated(ExampleObject2Handle())
|
||||
#define ExampleObject2InstUpdated(instId) UAVObjUpdated(ExampleObject2Handle(), instId)
|
||||
#define ExampleObject2GetMetadata(dataOut) UAVObjGetMetadata(ExampleObject2Handle(), dataOut)
|
||||
#define ExampleObject2SetMetadata(dataIn) UAVObjSetMetadata(ExampleObject2Handle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int8_t Field1;
|
||||
int16_t Field2;
|
||||
int32_t Field3;
|
||||
float Field4[4];
|
||||
|
||||
} __attribute__((packed)) ExampleObject2Data;
|
||||
|
||||
// Field information
|
||||
// Field Field1 information
|
||||
// Field Field2 information
|
||||
// Field Field3 information
|
||||
// Field Field4 information
|
||||
/* Number of elements for field Field4 */
|
||||
#define EXAMPLEOBJECT2_FIELD4_NUMELEM 4
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ExampleObject2Initialize();
|
||||
UAVObjHandle ExampleObject2Handle();
|
||||
|
||||
#endif // EXAMPLEOBJECT2_H
|
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file examplesettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ExampleSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: examplesettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef EXAMPLESETTINGS_H
|
||||
#define EXAMPLESETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define EXAMPLESETTINGS_OBJID 1640607828U
|
||||
#define EXAMPLESETTINGS_NAME "ExampleSettings"
|
||||
#define EXAMPLESETTINGS_METANAME "ExampleSettingsMeta"
|
||||
#define EXAMPLESETTINGS_ISSINGLEINST 1
|
||||
#define EXAMPLESETTINGS_ISSETTINGS 1
|
||||
#define EXAMPLESETTINGS_NUMBYTES sizeof(ExampleSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define ExampleSettingsGet(dataOut) UAVObjGetData(ExampleSettingsHandle(), dataOut)
|
||||
#define ExampleSettingsSet(dataIn) UAVObjSetData(ExampleSettingsHandle(), dataIn)
|
||||
#define ExampleSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(ExampleSettingsHandle(), instId, dataOut)
|
||||
#define ExampleSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(ExampleSettingsHandle(), instId, dataIn)
|
||||
#define ExampleSettingsConnectQueue(queue) UAVObjConnectQueue(ExampleSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleSettingsConnectCallback(cb) UAVObjConnectCallback(ExampleSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ExampleSettingsCreateInstance() UAVObjCreateInstance(ExampleSettingsHandle())
|
||||
#define ExampleSettingsRequestUpdate() UAVObjRequestUpdate(ExampleSettingsHandle())
|
||||
#define ExampleSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ExampleSettingsHandle(), instId)
|
||||
#define ExampleSettingsUpdated() UAVObjUpdated(ExampleSettingsHandle())
|
||||
#define ExampleSettingsInstUpdated(instId) UAVObjUpdated(ExampleSettingsHandle(), instId)
|
||||
#define ExampleSettingsGetMetadata(dataOut) UAVObjGetMetadata(ExampleSettingsHandle(), dataOut)
|
||||
#define ExampleSettingsSetMetadata(dataIn) UAVObjSetMetadata(ExampleSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int32_t UpdatePeriod;
|
||||
int32_t StepSize;
|
||||
uint8_t StepDirection;
|
||||
|
||||
} __attribute__((packed)) ExampleSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field UpdatePeriod information
|
||||
// Field StepSize information
|
||||
// Field StepDirection information
|
||||
/* Enumeration options for field StepDirection */
|
||||
typedef enum { EXAMPLESETTINGS_STEPDIRECTION_UP=0, EXAMPLESETTINGS_STEPDIRECTION_DOWN=1, } ExampleSettingsStepDirectionOptions;
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ExampleSettingsInitialize();
|
||||
UAVObjHandle ExampleSettingsHandle();
|
||||
|
||||
#endif // EXAMPLESETTINGS_H
|
@ -1,76 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file flightbatterystate.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the FlightBatteryState object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: flightbatterystate.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef FLIGHTBATTERYSTATE_H
|
||||
#define FLIGHTBATTERYSTATE_H
|
||||
|
||||
// Object constants
|
||||
#define FLIGHTBATTERYSTATE_OBJID 144318184U
|
||||
#define FLIGHTBATTERYSTATE_NAME "FlightBatteryState"
|
||||
#define FLIGHTBATTERYSTATE_METANAME "FlightBatteryStateMeta"
|
||||
#define FLIGHTBATTERYSTATE_ISSINGLEINST 1
|
||||
#define FLIGHTBATTERYSTATE_ISSETTINGS 0
|
||||
#define FLIGHTBATTERYSTATE_NUMBYTES sizeof(FlightBatteryStateData)
|
||||
|
||||
// Object access macros
|
||||
#define FlightBatteryStateGet(dataOut) UAVObjGetData(FlightBatteryStateHandle(), dataOut)
|
||||
#define FlightBatteryStateSet(dataIn) UAVObjSetData(FlightBatteryStateHandle(), dataIn)
|
||||
#define FlightBatteryStateInstGet(instId, dataOut) UAVObjGetInstanceData(FlightBatteryStateHandle(), instId, dataOut)
|
||||
#define FlightBatteryStateInstSet(instId, dataIn) UAVObjSetInstanceData(FlightBatteryStateHandle(), instId, dataIn)
|
||||
#define FlightBatteryStateConnectQueue(queue) UAVObjConnectQueue(FlightBatteryStateHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define FlightBatteryStateConnectCallback(cb) UAVObjConnectCallback(FlightBatteryStateHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define FlightBatteryStateCreateInstance() UAVObjCreateInstance(FlightBatteryStateHandle())
|
||||
#define FlightBatteryStateRequestUpdate() UAVObjRequestUpdate(FlightBatteryStateHandle())
|
||||
#define FlightBatteryStateRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(FlightBatteryStateHandle(), instId)
|
||||
#define FlightBatteryStateUpdated() UAVObjUpdated(FlightBatteryStateHandle())
|
||||
#define FlightBatteryStateInstUpdated(instId) UAVObjUpdated(FlightBatteryStateHandle(), instId)
|
||||
#define FlightBatteryStateGetMetadata(dataOut) UAVObjGetMetadata(FlightBatteryStateHandle(), dataOut)
|
||||
#define FlightBatteryStateSetMetadata(dataIn) UAVObjSetMetadata(FlightBatteryStateHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
float Voltage;
|
||||
float Current;
|
||||
uint32_t ConsumedEnergy;
|
||||
|
||||
} __attribute__((packed)) FlightBatteryStateData;
|
||||
|
||||
// Field information
|
||||
// Field Voltage information
|
||||
// Field Current information
|
||||
// Field ConsumedEnergy information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t FlightBatteryStateInitialize();
|
||||
UAVObjHandle FlightBatteryStateHandle();
|
||||
|
||||
#endif // FLIGHTBATTERYSTATE_H
|
@ -1,84 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file flighttelemetrystats.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the FlightTelemetryStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: flighttelemetrystats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef FLIGHTTELEMETRYSTATS_H
|
||||
#define FLIGHTTELEMETRYSTATS_H
|
||||
|
||||
// Object constants
|
||||
#define FLIGHTTELEMETRYSTATS_OBJID 1712072286U
|
||||
#define FLIGHTTELEMETRYSTATS_NAME "FlightTelemetryStats"
|
||||
#define FLIGHTTELEMETRYSTATS_METANAME "FlightTelemetryStatsMeta"
|
||||
#define FLIGHTTELEMETRYSTATS_ISSINGLEINST 1
|
||||
#define FLIGHTTELEMETRYSTATS_ISSETTINGS 0
|
||||
#define FLIGHTTELEMETRYSTATS_NUMBYTES sizeof(FlightTelemetryStatsData)
|
||||
|
||||
// Object access macros
|
||||
#define FlightTelemetryStatsGet(dataOut) UAVObjGetData(FlightTelemetryStatsHandle(), dataOut)
|
||||
#define FlightTelemetryStatsSet(dataIn) UAVObjSetData(FlightTelemetryStatsHandle(), dataIn)
|
||||
#define FlightTelemetryStatsInstGet(instId, dataOut) UAVObjGetInstanceData(FlightTelemetryStatsHandle(), instId, dataOut)
|
||||
#define FlightTelemetryStatsInstSet(instId, dataIn) UAVObjSetInstanceData(FlightTelemetryStatsHandle(), instId, dataIn)
|
||||
#define FlightTelemetryStatsConnectQueue(queue) UAVObjConnectQueue(FlightTelemetryStatsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define FlightTelemetryStatsConnectCallback(cb) UAVObjConnectCallback(FlightTelemetryStatsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define FlightTelemetryStatsCreateInstance() UAVObjCreateInstance(FlightTelemetryStatsHandle())
|
||||
#define FlightTelemetryStatsRequestUpdate() UAVObjRequestUpdate(FlightTelemetryStatsHandle())
|
||||
#define FlightTelemetryStatsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(FlightTelemetryStatsHandle(), instId)
|
||||
#define FlightTelemetryStatsUpdated() UAVObjUpdated(FlightTelemetryStatsHandle())
|
||||
#define FlightTelemetryStatsInstUpdated(instId) UAVObjUpdated(FlightTelemetryStatsHandle(), instId)
|
||||
#define FlightTelemetryStatsGetMetadata(dataOut) UAVObjGetMetadata(FlightTelemetryStatsHandle(), dataOut)
|
||||
#define FlightTelemetryStatsSetMetadata(dataIn) UAVObjSetMetadata(FlightTelemetryStatsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Status;
|
||||
float TxDataRate;
|
||||
float RxDataRate;
|
||||
uint32_t TxFailures;
|
||||
uint32_t RxFailures;
|
||||
uint32_t TxRetries;
|
||||
|
||||
} __attribute__((packed)) FlightTelemetryStatsData;
|
||||
|
||||
// Field information
|
||||
// Field Status information
|
||||
/* Enumeration options for field Status */
|
||||
typedef enum { FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED=0, FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEREQ=1, FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK=2, FLIGHTTELEMETRYSTATS_STATUS_CONNECTED=3, } FlightTelemetryStatsStatusOptions;
|
||||
// Field TxDataRate information
|
||||
// Field RxDataRate information
|
||||
// Field TxFailures information
|
||||
// Field RxFailures information
|
||||
// Field TxRetries information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t FlightTelemetryStatsInitialize();
|
||||
UAVObjHandle FlightTelemetryStatsHandle();
|
||||
|
||||
#endif // FLIGHTTELEMETRYSTATS_H
|
@ -1,84 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file gcstelemetrystats.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the GCSTelemetryStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: gcstelemetrystats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef GCSTELEMETRYSTATS_H
|
||||
#define GCSTELEMETRYSTATS_H
|
||||
|
||||
// Object constants
|
||||
#define GCSTELEMETRYSTATS_OBJID 1998458950U
|
||||
#define GCSTELEMETRYSTATS_NAME "GCSTelemetryStats"
|
||||
#define GCSTELEMETRYSTATS_METANAME "GCSTelemetryStatsMeta"
|
||||
#define GCSTELEMETRYSTATS_ISSINGLEINST 1
|
||||
#define GCSTELEMETRYSTATS_ISSETTINGS 0
|
||||
#define GCSTELEMETRYSTATS_NUMBYTES sizeof(GCSTelemetryStatsData)
|
||||
|
||||
// Object access macros
|
||||
#define GCSTelemetryStatsGet(dataOut) UAVObjGetData(GCSTelemetryStatsHandle(), dataOut)
|
||||
#define GCSTelemetryStatsSet(dataIn) UAVObjSetData(GCSTelemetryStatsHandle(), dataIn)
|
||||
#define GCSTelemetryStatsInstGet(instId, dataOut) UAVObjGetInstanceData(GCSTelemetryStatsHandle(), instId, dataOut)
|
||||
#define GCSTelemetryStatsInstSet(instId, dataIn) UAVObjSetInstanceData(GCSTelemetryStatsHandle(), instId, dataIn)
|
||||
#define GCSTelemetryStatsConnectQueue(queue) UAVObjConnectQueue(GCSTelemetryStatsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define GCSTelemetryStatsConnectCallback(cb) UAVObjConnectCallback(GCSTelemetryStatsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define GCSTelemetryStatsCreateInstance() UAVObjCreateInstance(GCSTelemetryStatsHandle())
|
||||
#define GCSTelemetryStatsRequestUpdate() UAVObjRequestUpdate(GCSTelemetryStatsHandle())
|
||||
#define GCSTelemetryStatsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(GCSTelemetryStatsHandle(), instId)
|
||||
#define GCSTelemetryStatsUpdated() UAVObjUpdated(GCSTelemetryStatsHandle())
|
||||
#define GCSTelemetryStatsInstUpdated(instId) UAVObjUpdated(GCSTelemetryStatsHandle(), instId)
|
||||
#define GCSTelemetryStatsGetMetadata(dataOut) UAVObjGetMetadata(GCSTelemetryStatsHandle(), dataOut)
|
||||
#define GCSTelemetryStatsSetMetadata(dataIn) UAVObjSetMetadata(GCSTelemetryStatsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Status;
|
||||
float TxDataRate;
|
||||
float RxDataRate;
|
||||
uint32_t TxFailures;
|
||||
uint32_t RxFailures;
|
||||
uint32_t TxRetries;
|
||||
|
||||
} __attribute__((packed)) GCSTelemetryStatsData;
|
||||
|
||||
// Field information
|
||||
// Field Status information
|
||||
/* Enumeration options for field Status */
|
||||
typedef enum { GCSTELEMETRYSTATS_STATUS_DISCONNECTED=0, GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ=1, GCSTELEMETRYSTATS_STATUS_HANDSHAKEACK=2, GCSTELEMETRYSTATS_STATUS_CONNECTED=3, } GCSTelemetryStatsStatusOptions;
|
||||
// Field TxDataRate information
|
||||
// Field RxDataRate information
|
||||
// Field TxFailures information
|
||||
// Field RxFailures information
|
||||
// Field TxRetries information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t GCSTelemetryStatsInitialize();
|
||||
UAVObjHandle GCSTelemetryStatsHandle();
|
||||
|
||||
#endif // GCSTELEMETRYSTATS_H
|
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file headingactual.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the HeadingActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: headingactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef HEADINGACTUAL_H
|
||||
#define HEADINGACTUAL_H
|
||||
|
||||
// Object constants
|
||||
#define HEADINGACTUAL_OBJID 2921442332U
|
||||
#define HEADINGACTUAL_NAME "HeadingActual"
|
||||
#define HEADINGACTUAL_METANAME "HeadingActualMeta"
|
||||
#define HEADINGACTUAL_ISSINGLEINST 1
|
||||
#define HEADINGACTUAL_ISSETTINGS 0
|
||||
#define HEADINGACTUAL_NUMBYTES sizeof(HeadingActualData)
|
||||
|
||||
// Object access macros
|
||||
#define HeadingActualGet(dataOut) UAVObjGetData(HeadingActualHandle(), dataOut)
|
||||
#define HeadingActualSet(dataIn) UAVObjSetData(HeadingActualHandle(), dataIn)
|
||||
#define HeadingActualInstGet(instId, dataOut) UAVObjGetInstanceData(HeadingActualHandle(), instId, dataOut)
|
||||
#define HeadingActualInstSet(instId, dataIn) UAVObjSetInstanceData(HeadingActualHandle(), instId, dataIn)
|
||||
#define HeadingActualConnectQueue(queue) UAVObjConnectQueue(HeadingActualHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define HeadingActualConnectCallback(cb) UAVObjConnectCallback(HeadingActualHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define HeadingActualCreateInstance() UAVObjCreateInstance(HeadingActualHandle())
|
||||
#define HeadingActualRequestUpdate() UAVObjRequestUpdate(HeadingActualHandle())
|
||||
#define HeadingActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(HeadingActualHandle(), instId)
|
||||
#define HeadingActualUpdated() UAVObjUpdated(HeadingActualHandle())
|
||||
#define HeadingActualInstUpdated(instId) UAVObjUpdated(HeadingActualHandle(), instId)
|
||||
#define HeadingActualGetMetadata(dataOut) UAVObjGetMetadata(HeadingActualHandle(), dataOut)
|
||||
#define HeadingActualSetMetadata(dataIn) UAVObjSetMetadata(HeadingActualHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
int16_t raw[3];
|
||||
float heading;
|
||||
|
||||
} __attribute__((packed)) HeadingActualData;
|
||||
|
||||
// Field information
|
||||
// Field raw information
|
||||
/* Array element names for field raw */
|
||||
typedef enum { HEADINGACTUAL_RAW_X=0, HEADINGACTUAL_RAW_Y=1, HEADINGACTUAL_RAW_Z=2, } HeadingActualrawElem;
|
||||
/* Number of elements for field raw */
|
||||
#define HEADINGACTUAL_RAW_NUMELEM 3
|
||||
// Field heading information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t HeadingActualInitialize();
|
||||
UAVObjHandle HeadingActualHandle();
|
||||
|
||||
#endif // HEADINGACTUAL_H
|
@ -1,90 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrolcommand.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ManualControlCommand object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: manualcontrolcommand.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef MANUALCONTROLCOMMAND_H
|
||||
#define MANUALCONTROLCOMMAND_H
|
||||
|
||||
// Object constants
|
||||
#define MANUALCONTROLCOMMAND_OBJID 990495372U
|
||||
#define MANUALCONTROLCOMMAND_NAME "ManualControlCommand"
|
||||
#define MANUALCONTROLCOMMAND_METANAME "ManualControlCommandMeta"
|
||||
#define MANUALCONTROLCOMMAND_ISSINGLEINST 1
|
||||
#define MANUALCONTROLCOMMAND_ISSETTINGS 0
|
||||
#define MANUALCONTROLCOMMAND_NUMBYTES sizeof(ManualControlCommandData)
|
||||
|
||||
// Object access macros
|
||||
#define ManualControlCommandGet(dataOut) UAVObjGetData(ManualControlCommandHandle(), dataOut)
|
||||
#define ManualControlCommandSet(dataIn) UAVObjSetData(ManualControlCommandHandle(), dataIn)
|
||||
#define ManualControlCommandInstGet(instId, dataOut) UAVObjGetInstanceData(ManualControlCommandHandle(), instId, dataOut)
|
||||
#define ManualControlCommandInstSet(instId, dataIn) UAVObjSetInstanceData(ManualControlCommandHandle(), instId, dataIn)
|
||||
#define ManualControlCommandConnectQueue(queue) UAVObjConnectQueue(ManualControlCommandHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ManualControlCommandConnectCallback(cb) UAVObjConnectCallback(ManualControlCommandHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ManualControlCommandCreateInstance() UAVObjCreateInstance(ManualControlCommandHandle())
|
||||
#define ManualControlCommandRequestUpdate() UAVObjRequestUpdate(ManualControlCommandHandle())
|
||||
#define ManualControlCommandRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ManualControlCommandHandle(), instId)
|
||||
#define ManualControlCommandUpdated() UAVObjUpdated(ManualControlCommandHandle())
|
||||
#define ManualControlCommandInstUpdated(instId) UAVObjUpdated(ManualControlCommandHandle(), instId)
|
||||
#define ManualControlCommandGetMetadata(dataOut) UAVObjGetMetadata(ManualControlCommandHandle(), dataOut)
|
||||
#define ManualControlCommandSetMetadata(dataIn) UAVObjSetMetadata(ManualControlCommandHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Connected;
|
||||
float Roll;
|
||||
float Pitch;
|
||||
float Yaw;
|
||||
float Throttle;
|
||||
uint8_t FlightMode;
|
||||
int16_t Channel[8];
|
||||
|
||||
} __attribute__((packed)) ManualControlCommandData;
|
||||
|
||||
// Field information
|
||||
// Field Connected information
|
||||
/* Enumeration options for field Connected */
|
||||
typedef enum { MANUALCONTROLCOMMAND_CONNECTED_FALSE=0, MANUALCONTROLCOMMAND_CONNECTED_TRUE=1, } ManualControlCommandConnectedOptions;
|
||||
// Field Roll information
|
||||
// Field Pitch information
|
||||
// Field Yaw information
|
||||
// Field Throttle information
|
||||
// Field FlightMode information
|
||||
/* Enumeration options for field FlightMode */
|
||||
typedef enum { MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL=0, MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED=1, MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO=2, } ManualControlCommandFlightModeOptions;
|
||||
// Field Channel information
|
||||
/* Number of elements for field Channel */
|
||||
#define MANUALCONTROLCOMMAND_CHANNEL_NUMELEM 8
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ManualControlCommandInitialize();
|
||||
UAVObjHandle ManualControlCommandHandle();
|
||||
|
||||
#endif // MANUALCONTROLCOMMAND_H
|
@ -1,106 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrolsettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ManualControlSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: manualcontrolsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef MANUALCONTROLSETTINGS_H
|
||||
#define MANUALCONTROLSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define MANUALCONTROLSETTINGS_OBJID 2933673028U
|
||||
#define MANUALCONTROLSETTINGS_NAME "ManualControlSettings"
|
||||
#define MANUALCONTROLSETTINGS_METANAME "ManualControlSettingsMeta"
|
||||
#define MANUALCONTROLSETTINGS_ISSINGLEINST 1
|
||||
#define MANUALCONTROLSETTINGS_ISSETTINGS 1
|
||||
#define MANUALCONTROLSETTINGS_NUMBYTES sizeof(ManualControlSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define ManualControlSettingsGet(dataOut) UAVObjGetData(ManualControlSettingsHandle(), dataOut)
|
||||
#define ManualControlSettingsSet(dataIn) UAVObjSetData(ManualControlSettingsHandle(), dataIn)
|
||||
#define ManualControlSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(ManualControlSettingsHandle(), instId, dataOut)
|
||||
#define ManualControlSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(ManualControlSettingsHandle(), instId, dataIn)
|
||||
#define ManualControlSettingsConnectQueue(queue) UAVObjConnectQueue(ManualControlSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ManualControlSettingsConnectCallback(cb) UAVObjConnectCallback(ManualControlSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ManualControlSettingsCreateInstance() UAVObjCreateInstance(ManualControlSettingsHandle())
|
||||
#define ManualControlSettingsRequestUpdate() UAVObjRequestUpdate(ManualControlSettingsHandle())
|
||||
#define ManualControlSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ManualControlSettingsHandle(), instId)
|
||||
#define ManualControlSettingsUpdated() UAVObjUpdated(ManualControlSettingsHandle())
|
||||
#define ManualControlSettingsInstUpdated(instId) UAVObjUpdated(ManualControlSettingsHandle(), instId)
|
||||
#define ManualControlSettingsGetMetadata(dataOut) UAVObjGetMetadata(ManualControlSettingsHandle(), dataOut)
|
||||
#define ManualControlSettingsSetMetadata(dataIn) UAVObjSetMetadata(ManualControlSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t InputMode;
|
||||
uint8_t Roll;
|
||||
uint8_t Pitch;
|
||||
uint8_t Yaw;
|
||||
uint8_t Throttle;
|
||||
uint8_t FlightMode;
|
||||
int16_t ChannelMax[8];
|
||||
int16_t ChannelNeutral[8];
|
||||
int16_t ChannelMin[8];
|
||||
|
||||
} __attribute__((packed)) ManualControlSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field InputMode information
|
||||
/* Enumeration options for field InputMode */
|
||||
typedef enum { MANUALCONTROLSETTINGS_INPUTMODE_PWM=0, MANUALCONTROLSETTINGS_INPUTMODE_PPM=1, MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM=2, } ManualControlSettingsInputModeOptions;
|
||||
// Field Roll information
|
||||
/* Enumeration options for field Roll */
|
||||
typedef enum { MANUALCONTROLSETTINGS_ROLL_CHANNEL0=0, MANUALCONTROLSETTINGS_ROLL_CHANNEL1=1, MANUALCONTROLSETTINGS_ROLL_CHANNEL2=2, MANUALCONTROLSETTINGS_ROLL_CHANNEL3=3, MANUALCONTROLSETTINGS_ROLL_CHANNEL4=4, MANUALCONTROLSETTINGS_ROLL_CHANNEL5=5, MANUALCONTROLSETTINGS_ROLL_CHANNEL6=6, MANUALCONTROLSETTINGS_ROLL_CHANNEL7=7, MANUALCONTROLSETTINGS_ROLL_NONE=8, } ManualControlSettingsRollOptions;
|
||||
// Field Pitch information
|
||||
/* Enumeration options for field Pitch */
|
||||
typedef enum { MANUALCONTROLSETTINGS_PITCH_CHANNEL0=0, MANUALCONTROLSETTINGS_PITCH_CHANNEL1=1, MANUALCONTROLSETTINGS_PITCH_CHANNEL2=2, MANUALCONTROLSETTINGS_PITCH_CHANNEL3=3, MANUALCONTROLSETTINGS_PITCH_CHANNEL4=4, MANUALCONTROLSETTINGS_PITCH_CHANNEL5=5, MANUALCONTROLSETTINGS_PITCH_CHANNEL6=6, MANUALCONTROLSETTINGS_PITCH_CHANNEL7=7, MANUALCONTROLSETTINGS_PITCH_NONE=8, } ManualControlSettingsPitchOptions;
|
||||
// Field Yaw information
|
||||
/* Enumeration options for field Yaw */
|
||||
typedef enum { MANUALCONTROLSETTINGS_YAW_CHANNEL0=0, MANUALCONTROLSETTINGS_YAW_CHANNEL1=1, MANUALCONTROLSETTINGS_YAW_CHANNEL2=2, MANUALCONTROLSETTINGS_YAW_CHANNEL3=3, MANUALCONTROLSETTINGS_YAW_CHANNEL4=4, MANUALCONTROLSETTINGS_YAW_CHANNEL5=5, MANUALCONTROLSETTINGS_YAW_CHANNEL6=6, MANUALCONTROLSETTINGS_YAW_CHANNEL7=7, MANUALCONTROLSETTINGS_YAW_NONE=8, } ManualControlSettingsYawOptions;
|
||||
// Field Throttle information
|
||||
/* Enumeration options for field Throttle */
|
||||
typedef enum { MANUALCONTROLSETTINGS_THROTTLE_CHANNEL0=0, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL1=1, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL2=2, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL3=3, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL4=4, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL5=5, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL6=6, MANUALCONTROLSETTINGS_THROTTLE_CHANNEL7=7, MANUALCONTROLSETTINGS_THROTTLE_NONE=8, } ManualControlSettingsThrottleOptions;
|
||||
// Field FlightMode information
|
||||
/* Enumeration options for field FlightMode */
|
||||
typedef enum { MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL0=0, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL1=1, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL2=2, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL3=3, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL4=4, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL5=5, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL6=6, MANUALCONTROLSETTINGS_FLIGHTMODE_CHANNEL7=7, MANUALCONTROLSETTINGS_FLIGHTMODE_NONE=8, } ManualControlSettingsFlightModeOptions;
|
||||
// Field ChannelMax information
|
||||
/* Number of elements for field ChannelMax */
|
||||
#define MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM 8
|
||||
// Field ChannelNeutral information
|
||||
/* Number of elements for field ChannelNeutral */
|
||||
#define MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM 8
|
||||
// Field ChannelMin information
|
||||
/* Number of elements for field ChannelMin */
|
||||
#define MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM 8
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ManualControlSettingsInitialize();
|
||||
UAVObjHandle ManualControlSettingsHandle();
|
||||
|
||||
#endif // MANUALCONTROLSETTINGS_H
|
@ -1,82 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file objectpersistence.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ObjectPersistence object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: objectpersistence.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef OBJECTPERSISTENCE_H
|
||||
#define OBJECTPERSISTENCE_H
|
||||
|
||||
// Object constants
|
||||
#define OBJECTPERSISTENCE_OBJID 572614706U
|
||||
#define OBJECTPERSISTENCE_NAME "ObjectPersistence"
|
||||
#define OBJECTPERSISTENCE_METANAME "ObjectPersistenceMeta"
|
||||
#define OBJECTPERSISTENCE_ISSINGLEINST 1
|
||||
#define OBJECTPERSISTENCE_ISSETTINGS 0
|
||||
#define OBJECTPERSISTENCE_NUMBYTES sizeof(ObjectPersistenceData)
|
||||
|
||||
// Object access macros
|
||||
#define ObjectPersistenceGet(dataOut) UAVObjGetData(ObjectPersistenceHandle(), dataOut)
|
||||
#define ObjectPersistenceSet(dataIn) UAVObjSetData(ObjectPersistenceHandle(), dataIn)
|
||||
#define ObjectPersistenceInstGet(instId, dataOut) UAVObjGetInstanceData(ObjectPersistenceHandle(), instId, dataOut)
|
||||
#define ObjectPersistenceInstSet(instId, dataIn) UAVObjSetInstanceData(ObjectPersistenceHandle(), instId, dataIn)
|
||||
#define ObjectPersistenceConnectQueue(queue) UAVObjConnectQueue(ObjectPersistenceHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define ObjectPersistenceConnectCallback(cb) UAVObjConnectCallback(ObjectPersistenceHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define ObjectPersistenceCreateInstance() UAVObjCreateInstance(ObjectPersistenceHandle())
|
||||
#define ObjectPersistenceRequestUpdate() UAVObjRequestUpdate(ObjectPersistenceHandle())
|
||||
#define ObjectPersistenceRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(ObjectPersistenceHandle(), instId)
|
||||
#define ObjectPersistenceUpdated() UAVObjUpdated(ObjectPersistenceHandle())
|
||||
#define ObjectPersistenceInstUpdated(instId) UAVObjUpdated(ObjectPersistenceHandle(), instId)
|
||||
#define ObjectPersistenceGetMetadata(dataOut) UAVObjGetMetadata(ObjectPersistenceHandle(), dataOut)
|
||||
#define ObjectPersistenceSetMetadata(dataIn) UAVObjSetMetadata(ObjectPersistenceHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Operation;
|
||||
uint8_t Selection;
|
||||
uint32_t ObjectID;
|
||||
uint32_t InstanceID;
|
||||
|
||||
} __attribute__((packed)) ObjectPersistenceData;
|
||||
|
||||
// Field information
|
||||
// Field Operation information
|
||||
/* Enumeration options for field Operation */
|
||||
typedef enum { OBJECTPERSISTENCE_OPERATION_LOAD=0, OBJECTPERSISTENCE_OPERATION_SAVE=1, OBJECTPERSISTENCE_OPERATION_DELETE=2, } ObjectPersistenceOperationOptions;
|
||||
// Field Selection information
|
||||
/* Enumeration options for field Selection */
|
||||
typedef enum { OBJECTPERSISTENCE_SELECTION_SINGLEOBJECT=0, OBJECTPERSISTENCE_SELECTION_ALLSETTINGS=1, OBJECTPERSISTENCE_SELECTION_ALLMETAOBJECTS=2, OBJECTPERSISTENCE_SELECTION_ALLOBJECTS=3, } ObjectPersistenceSelectionOptions;
|
||||
// Field ObjectID information
|
||||
// Field InstanceID information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t ObjectPersistenceInitialize();
|
||||
UAVObjHandle ObjectPersistenceHandle();
|
||||
|
||||
#endif // OBJECTPERSISTENCE_H
|
@ -1,94 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file positionactual.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the PositionActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: positionactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef POSITIONACTUAL_H
|
||||
#define POSITIONACTUAL_H
|
||||
|
||||
// Object constants
|
||||
#define POSITIONACTUAL_OBJID 1265479538U
|
||||
#define POSITIONACTUAL_NAME "PositionActual"
|
||||
#define POSITIONACTUAL_METANAME "PositionActualMeta"
|
||||
#define POSITIONACTUAL_ISSINGLEINST 1
|
||||
#define POSITIONACTUAL_ISSETTINGS 0
|
||||
#define POSITIONACTUAL_NUMBYTES sizeof(PositionActualData)
|
||||
|
||||
// Object access macros
|
||||
#define PositionActualGet(dataOut) UAVObjGetData(PositionActualHandle(), dataOut)
|
||||
#define PositionActualSet(dataIn) UAVObjSetData(PositionActualHandle(), dataIn)
|
||||
#define PositionActualInstGet(instId, dataOut) UAVObjGetInstanceData(PositionActualHandle(), instId, dataOut)
|
||||
#define PositionActualInstSet(instId, dataIn) UAVObjSetInstanceData(PositionActualHandle(), instId, dataIn)
|
||||
#define PositionActualConnectQueue(queue) UAVObjConnectQueue(PositionActualHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define PositionActualConnectCallback(cb) UAVObjConnectCallback(PositionActualHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define PositionActualCreateInstance() UAVObjCreateInstance(PositionActualHandle())
|
||||
#define PositionActualRequestUpdate() UAVObjRequestUpdate(PositionActualHandle())
|
||||
#define PositionActualRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(PositionActualHandle(), instId)
|
||||
#define PositionActualUpdated() UAVObjUpdated(PositionActualHandle())
|
||||
#define PositionActualInstUpdated(instId) UAVObjUpdated(PositionActualHandle(), instId)
|
||||
#define PositionActualGetMetadata(dataOut) UAVObjGetMetadata(PositionActualHandle(), dataOut)
|
||||
#define PositionActualSetMetadata(dataIn) UAVObjSetMetadata(PositionActualHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Status;
|
||||
float Latitude;
|
||||
float Longitude;
|
||||
float Altitude;
|
||||
float GeoidSeparation;
|
||||
float Heading;
|
||||
float Groundspeed;
|
||||
int8_t Satellites;
|
||||
float PDOP;
|
||||
float HDOP;
|
||||
float VDOP;
|
||||
|
||||
} __attribute__((packed)) PositionActualData;
|
||||
|
||||
// Field information
|
||||
// Field Status information
|
||||
/* Enumeration options for field Status */
|
||||
typedef enum { POSITIONACTUAL_STATUS_NOGPS=0, POSITIONACTUAL_STATUS_NOFIX=1, POSITIONACTUAL_STATUS_FIX2D=2, POSITIONACTUAL_STATUS_FIX3D=3, } PositionActualStatusOptions;
|
||||
// Field Latitude information
|
||||
// Field Longitude information
|
||||
// Field Altitude information
|
||||
// Field GeoidSeparation information
|
||||
// Field Heading information
|
||||
// Field Groundspeed information
|
||||
// Field Satellites information
|
||||
// Field PDOP information
|
||||
// Field HDOP information
|
||||
// Field VDOP information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t PositionActualInitialize();
|
||||
UAVObjHandle PositionActualHandle();
|
||||
|
||||
#endif // POSITIONACTUAL_H
|
@ -1,96 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file stabilizationsettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the StabilizationSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: stabilizationsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef STABILIZATIONSETTINGS_H
|
||||
#define STABILIZATIONSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define STABILIZATIONSETTINGS_OBJID 2185681924U
|
||||
#define STABILIZATIONSETTINGS_NAME "StabilizationSettings"
|
||||
#define STABILIZATIONSETTINGS_METANAME "StabilizationSettingsMeta"
|
||||
#define STABILIZATIONSETTINGS_ISSINGLEINST 1
|
||||
#define STABILIZATIONSETTINGS_ISSETTINGS 1
|
||||
#define STABILIZATIONSETTINGS_NUMBYTES sizeof(StabilizationSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define StabilizationSettingsGet(dataOut) UAVObjGetData(StabilizationSettingsHandle(), dataOut)
|
||||
#define StabilizationSettingsSet(dataIn) UAVObjSetData(StabilizationSettingsHandle(), dataIn)
|
||||
#define StabilizationSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(StabilizationSettingsHandle(), instId, dataOut)
|
||||
#define StabilizationSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(StabilizationSettingsHandle(), instId, dataIn)
|
||||
#define StabilizationSettingsConnectQueue(queue) UAVObjConnectQueue(StabilizationSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define StabilizationSettingsConnectCallback(cb) UAVObjConnectCallback(StabilizationSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define StabilizationSettingsCreateInstance() UAVObjCreateInstance(StabilizationSettingsHandle())
|
||||
#define StabilizationSettingsRequestUpdate() UAVObjRequestUpdate(StabilizationSettingsHandle())
|
||||
#define StabilizationSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(StabilizationSettingsHandle(), instId)
|
||||
#define StabilizationSettingsUpdated() UAVObjUpdated(StabilizationSettingsHandle())
|
||||
#define StabilizationSettingsInstUpdated(instId) UAVObjUpdated(StabilizationSettingsHandle(), instId)
|
||||
#define StabilizationSettingsGetMetadata(dataOut) UAVObjGetMetadata(StabilizationSettingsHandle(), dataOut)
|
||||
#define StabilizationSettingsSetMetadata(dataIn) UAVObjSetMetadata(StabilizationSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint16_t UpdatePeriod;
|
||||
float RollMax;
|
||||
float PitchMax;
|
||||
float ThrottleMax;
|
||||
float PitchKp;
|
||||
float PitchKi;
|
||||
float PitchKd;
|
||||
float RollKp;
|
||||
float RollKi;
|
||||
float RollKd;
|
||||
float YawKp;
|
||||
float YawKi;
|
||||
float YawKd;
|
||||
|
||||
} __attribute__((packed)) StabilizationSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field UpdatePeriod information
|
||||
// Field RollMax information
|
||||
// Field PitchMax information
|
||||
// Field ThrottleMax information
|
||||
// Field PitchKp information
|
||||
// Field PitchKi information
|
||||
// Field PitchKd information
|
||||
// Field RollKp information
|
||||
// Field RollKi information
|
||||
// Field RollKd information
|
||||
// Field YawKp information
|
||||
// Field YawKi information
|
||||
// Field YawKd information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t StabilizationSettingsInitialize();
|
||||
UAVObjHandle StabilizationSettingsHandle();
|
||||
|
||||
#endif // STABILIZATIONSETTINGS_H
|
@ -1,78 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemalarms.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemAlarms object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemalarms.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef SYSTEMALARMS_H
|
||||
#define SYSTEMALARMS_H
|
||||
|
||||
// Object constants
|
||||
#define SYSTEMALARMS_OBJID 2311311584U
|
||||
#define SYSTEMALARMS_NAME "SystemAlarms"
|
||||
#define SYSTEMALARMS_METANAME "SystemAlarmsMeta"
|
||||
#define SYSTEMALARMS_ISSINGLEINST 1
|
||||
#define SYSTEMALARMS_ISSETTINGS 0
|
||||
#define SYSTEMALARMS_NUMBYTES sizeof(SystemAlarmsData)
|
||||
|
||||
// Object access macros
|
||||
#define SystemAlarmsGet(dataOut) UAVObjGetData(SystemAlarmsHandle(), dataOut)
|
||||
#define SystemAlarmsSet(dataIn) UAVObjSetData(SystemAlarmsHandle(), dataIn)
|
||||
#define SystemAlarmsInstGet(instId, dataOut) UAVObjGetInstanceData(SystemAlarmsHandle(), instId, dataOut)
|
||||
#define SystemAlarmsInstSet(instId, dataIn) UAVObjSetInstanceData(SystemAlarmsHandle(), instId, dataIn)
|
||||
#define SystemAlarmsConnectQueue(queue) UAVObjConnectQueue(SystemAlarmsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define SystemAlarmsConnectCallback(cb) UAVObjConnectCallback(SystemAlarmsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define SystemAlarmsCreateInstance() UAVObjCreateInstance(SystemAlarmsHandle())
|
||||
#define SystemAlarmsRequestUpdate() UAVObjRequestUpdate(SystemAlarmsHandle())
|
||||
#define SystemAlarmsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(SystemAlarmsHandle(), instId)
|
||||
#define SystemAlarmsUpdated() UAVObjUpdated(SystemAlarmsHandle())
|
||||
#define SystemAlarmsInstUpdated(instId) UAVObjUpdated(SystemAlarmsHandle(), instId)
|
||||
#define SystemAlarmsGetMetadata(dataOut) UAVObjGetMetadata(SystemAlarmsHandle(), dataOut)
|
||||
#define SystemAlarmsSetMetadata(dataIn) UAVObjSetMetadata(SystemAlarmsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Alarm[10];
|
||||
|
||||
} __attribute__((packed)) SystemAlarmsData;
|
||||
|
||||
// Field information
|
||||
// Field Alarm information
|
||||
/* Enumeration options for field Alarm */
|
||||
typedef enum { SYSTEMALARMS_ALARM_OK=0, SYSTEMALARMS_ALARM_WARNING=1, SYSTEMALARMS_ALARM_ERROR=2, SYSTEMALARMS_ALARM_CRITICAL=3, } SystemAlarmsAlarmOptions;
|
||||
/* Array element names for field Alarm */
|
||||
typedef enum { SYSTEMALARMS_ALARM_OUTOFMEMORY=0, SYSTEMALARMS_ALARM_STACKOVERFLOW=1, SYSTEMALARMS_ALARM_CPUOVERLOAD=2, SYSTEMALARMS_ALARM_EVENTSYSTEM=3, SYSTEMALARMS_ALARM_SDCARD=4, SYSTEMALARMS_ALARM_TELEMETRY=5, SYSTEMALARMS_ALARM_MANUALCONTROL=6, SYSTEMALARMS_ALARM_ACTUATOR=7, SYSTEMALARMS_ALARM_STABILIZATION=8, SYSTEMALARMS_ALARM_AHRSCOMMS=9, } SystemAlarmsAlarmElem;
|
||||
/* Number of elements for field Alarm */
|
||||
#define SYSTEMALARMS_ALARM_NUMELEM 10
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t SystemAlarmsInitialize();
|
||||
UAVObjHandle SystemAlarmsHandle();
|
||||
|
||||
#endif // SYSTEMALARMS_H
|
@ -1,74 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemsettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef SYSTEMSETTINGS_H
|
||||
#define SYSTEMSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define SYSTEMSETTINGS_OBJID 59202798U
|
||||
#define SYSTEMSETTINGS_NAME "SystemSettings"
|
||||
#define SYSTEMSETTINGS_METANAME "SystemSettingsMeta"
|
||||
#define SYSTEMSETTINGS_ISSINGLEINST 1
|
||||
#define SYSTEMSETTINGS_ISSETTINGS 1
|
||||
#define SYSTEMSETTINGS_NUMBYTES sizeof(SystemSettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define SystemSettingsGet(dataOut) UAVObjGetData(SystemSettingsHandle(), dataOut)
|
||||
#define SystemSettingsSet(dataIn) UAVObjSetData(SystemSettingsHandle(), dataIn)
|
||||
#define SystemSettingsInstGet(instId, dataOut) UAVObjGetInstanceData(SystemSettingsHandle(), instId, dataOut)
|
||||
#define SystemSettingsInstSet(instId, dataIn) UAVObjSetInstanceData(SystemSettingsHandle(), instId, dataIn)
|
||||
#define SystemSettingsConnectQueue(queue) UAVObjConnectQueue(SystemSettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define SystemSettingsConnectCallback(cb) UAVObjConnectCallback(SystemSettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define SystemSettingsCreateInstance() UAVObjCreateInstance(SystemSettingsHandle())
|
||||
#define SystemSettingsRequestUpdate() UAVObjRequestUpdate(SystemSettingsHandle())
|
||||
#define SystemSettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(SystemSettingsHandle(), instId)
|
||||
#define SystemSettingsUpdated() UAVObjUpdated(SystemSettingsHandle())
|
||||
#define SystemSettingsInstUpdated(instId) UAVObjUpdated(SystemSettingsHandle(), instId)
|
||||
#define SystemSettingsGetMetadata(dataOut) UAVObjGetMetadata(SystemSettingsHandle(), dataOut)
|
||||
#define SystemSettingsSetMetadata(dataIn) UAVObjSetMetadata(SystemSettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t AirframeType;
|
||||
|
||||
} __attribute__((packed)) SystemSettingsData;
|
||||
|
||||
// Field information
|
||||
// Field AirframeType information
|
||||
/* Enumeration options for field AirframeType */
|
||||
typedef enum { SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING=0, SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON=1, SYSTEMSETTINGS_AIRFRAMETYPE_VTOL=2, } SystemSettingsAirframeTypeOptions;
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t SystemSettingsInitialize();
|
||||
UAVObjHandle SystemSettingsHandle();
|
||||
|
||||
#endif // SYSTEMSETTINGS_H
|
@ -1,76 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemstats.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemstats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef SYSTEMSTATS_H
|
||||
#define SYSTEMSTATS_H
|
||||
|
||||
// Object constants
|
||||
#define SYSTEMSTATS_OBJID 680908530U
|
||||
#define SYSTEMSTATS_NAME "SystemStats"
|
||||
#define SYSTEMSTATS_METANAME "SystemStatsMeta"
|
||||
#define SYSTEMSTATS_ISSINGLEINST 1
|
||||
#define SYSTEMSTATS_ISSETTINGS 0
|
||||
#define SYSTEMSTATS_NUMBYTES sizeof(SystemStatsData)
|
||||
|
||||
// Object access macros
|
||||
#define SystemStatsGet(dataOut) UAVObjGetData(SystemStatsHandle(), dataOut)
|
||||
#define SystemStatsSet(dataIn) UAVObjSetData(SystemStatsHandle(), dataIn)
|
||||
#define SystemStatsInstGet(instId, dataOut) UAVObjGetInstanceData(SystemStatsHandle(), instId, dataOut)
|
||||
#define SystemStatsInstSet(instId, dataIn) UAVObjSetInstanceData(SystemStatsHandle(), instId, dataIn)
|
||||
#define SystemStatsConnectQueue(queue) UAVObjConnectQueue(SystemStatsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define SystemStatsConnectCallback(cb) UAVObjConnectCallback(SystemStatsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define SystemStatsCreateInstance() UAVObjCreateInstance(SystemStatsHandle())
|
||||
#define SystemStatsRequestUpdate() UAVObjRequestUpdate(SystemStatsHandle())
|
||||
#define SystemStatsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(SystemStatsHandle(), instId)
|
||||
#define SystemStatsUpdated() UAVObjUpdated(SystemStatsHandle())
|
||||
#define SystemStatsInstUpdated(instId) UAVObjUpdated(SystemStatsHandle(), instId)
|
||||
#define SystemStatsGetMetadata(dataOut) UAVObjGetMetadata(SystemStatsHandle(), dataOut)
|
||||
#define SystemStatsSetMetadata(dataIn) UAVObjSetMetadata(SystemStatsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint32_t FlightTime;
|
||||
uint16_t HeapRemaining;
|
||||
uint8_t CPULoad;
|
||||
|
||||
} __attribute__((packed)) SystemStatsData;
|
||||
|
||||
// Field information
|
||||
// Field FlightTime information
|
||||
// Field HeapRemaining information
|
||||
// Field CPULoad information
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t SystemStatsInitialize();
|
||||
UAVObjHandle SystemStatsHandle();
|
||||
|
||||
#endif // SYSTEMSTATS_H
|
@ -1,74 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file telemetrysettings.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the TelemetrySettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: telemetrysettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef TELEMETRYSETTINGS_H
|
||||
#define TELEMETRYSETTINGS_H
|
||||
|
||||
// Object constants
|
||||
#define TELEMETRYSETTINGS_OBJID 2785592614U
|
||||
#define TELEMETRYSETTINGS_NAME "TelemetrySettings"
|
||||
#define TELEMETRYSETTINGS_METANAME "TelemetrySettingsMeta"
|
||||
#define TELEMETRYSETTINGS_ISSINGLEINST 1
|
||||
#define TELEMETRYSETTINGS_ISSETTINGS 1
|
||||
#define TELEMETRYSETTINGS_NUMBYTES sizeof(TelemetrySettingsData)
|
||||
|
||||
// Object access macros
|
||||
#define TelemetrySettingsGet(dataOut) UAVObjGetData(TelemetrySettingsHandle(), dataOut)
|
||||
#define TelemetrySettingsSet(dataIn) UAVObjSetData(TelemetrySettingsHandle(), dataIn)
|
||||
#define TelemetrySettingsInstGet(instId, dataOut) UAVObjGetInstanceData(TelemetrySettingsHandle(), instId, dataOut)
|
||||
#define TelemetrySettingsInstSet(instId, dataIn) UAVObjSetInstanceData(TelemetrySettingsHandle(), instId, dataIn)
|
||||
#define TelemetrySettingsConnectQueue(queue) UAVObjConnectQueue(TelemetrySettingsHandle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define TelemetrySettingsConnectCallback(cb) UAVObjConnectCallback(TelemetrySettingsHandle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define TelemetrySettingsCreateInstance() UAVObjCreateInstance(TelemetrySettingsHandle())
|
||||
#define TelemetrySettingsRequestUpdate() UAVObjRequestUpdate(TelemetrySettingsHandle())
|
||||
#define TelemetrySettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(TelemetrySettingsHandle(), instId)
|
||||
#define TelemetrySettingsUpdated() UAVObjUpdated(TelemetrySettingsHandle())
|
||||
#define TelemetrySettingsInstUpdated(instId) UAVObjUpdated(TelemetrySettingsHandle(), instId)
|
||||
#define TelemetrySettingsGetMetadata(dataOut) UAVObjGetMetadata(TelemetrySettingsHandle(), dataOut)
|
||||
#define TelemetrySettingsSetMetadata(dataIn) UAVObjSetMetadata(TelemetrySettingsHandle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t Speed;
|
||||
|
||||
} __attribute__((packed)) TelemetrySettingsData;
|
||||
|
||||
// Field information
|
||||
// Field Speed information
|
||||
/* Enumeration options for field Speed */
|
||||
typedef enum { TELEMETRYSETTINGS_SPEED_9600=0, TELEMETRYSETTINGS_SPEED_38400=1, TELEMETRYSETTINGS_SPEED_57600=2, } TelemetrySettingsSpeedOptions;
|
||||
|
||||
|
||||
// Generic interface functions
|
||||
int32_t TelemetrySettingsInitialize();
|
||||
UAVObjHandle TelemetrySettingsHandle();
|
||||
|
||||
#endif // TELEMETRYSETTINGS_H
|
@ -1,158 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file uavobjectmanager.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Include files of the uavobjectlist library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef UAVOBJECTMANAGER_H
|
||||
#define UAVOBJECTMANAGER_H
|
||||
|
||||
#define UAVOBJ_ALL_INSTANCES 0xFFFF
|
||||
#define UAVOBJ_MAX_INSTANCES 1000
|
||||
|
||||
typedef void* UAVObjHandle;
|
||||
|
||||
/**
|
||||
* Object update mode, used by multiple modules (e.g. telemetry and logger)
|
||||
*/
|
||||
typedef enum {
|
||||
UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */
|
||||
UPDATEMODE_ONCHANGE = 1, /** Only update object when its data changes */
|
||||
UPDATEMODE_MANUAL = 2, /** Manually update object, by calling the updated() function */
|
||||
UPDATEMODE_NEVER = 3 /** Object is never updated */
|
||||
} UAVObjUpdateMode;
|
||||
|
||||
/**
|
||||
* Object metadata, each object has a meta object that holds its metadata. The metadata define
|
||||
* properties for each object and can be used by multiple modules (e.g. telemetry and logger)
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t access; /** Defines the access level for the local transactions (readonly and readwrite) */
|
||||
uint8_t gcsAccess; /** Defines the access level for the local GCS transactions (readonly and readwrite), not used in the flight s/w */
|
||||
uint8_t telemetryAcked; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */
|
||||
uint8_t telemetryUpdateMode; /** Update mode used by the telemetry module (UAVObjUpdateMode) */
|
||||
int32_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */
|
||||
uint8_t gcsTelemetryAcked; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */
|
||||
uint8_t gcsTelemetryUpdateMode; /** Update mode used by the GCS (UAVObjUpdateMode) */
|
||||
int32_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */
|
||||
uint8_t loggingUpdateMode; /** Update mode used by the logging module (UAVObjUpdateMode) */
|
||||
uint32_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */
|
||||
} __attribute__((packed)) UAVObjMetadata;
|
||||
|
||||
/**
|
||||
* Event types generated by the objects.
|
||||
*/
|
||||
typedef enum {
|
||||
EV_UNPACKED = 0x01, /** Object data updated by unpacking */
|
||||
EV_UPDATED = 0x02, /** Object data updated by changing the data structure */
|
||||
EV_UPDATED_MANUAL = 0x04, /** Object update event manually generated */
|
||||
EV_UPDATE_REQ = 0x08 /** Request to update object data */
|
||||
} UAVObjEventType;
|
||||
|
||||
/**
|
||||
* Helper macros for event masks
|
||||
*/
|
||||
#define EV_MASK_ALL 0
|
||||
#define EV_MASK_ALL_UPDATES (EV_UNPACKED | EV_UPDATED | EV_UPDATED_MANUAL)
|
||||
|
||||
/**
|
||||
* Access types
|
||||
*/
|
||||
typedef enum {
|
||||
ACCESS_READWRITE = 0,
|
||||
ACCESS_READONLY = 1
|
||||
} UAVObjAccessType;
|
||||
|
||||
/**
|
||||
* Event message, this structure is sent in the event queue each time an event is generated
|
||||
*/
|
||||
typedef struct {
|
||||
UAVObjHandle obj;
|
||||
uint16_t instId;
|
||||
UAVObjEventType event;
|
||||
} UAVObjEvent;
|
||||
|
||||
/**
|
||||
* Event callback, this function is called when an event is invoked. The function
|
||||
* will be executed in the event task. The ev parameter should be copied if needed
|
||||
* after the function returns.
|
||||
*/
|
||||
typedef void (*UAVObjEventCallback)(UAVObjEvent* ev);
|
||||
|
||||
/**
|
||||
* Callback used to initialize the object fields to their default values.
|
||||
*/
|
||||
typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Event manager statistics
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t eventErrors;
|
||||
} UAVObjStats;
|
||||
|
||||
int32_t UAVObjInitialize();
|
||||
void UAVObjGetStats(UAVObjStats* statsOut);
|
||||
void UAVObjClearStats();
|
||||
UAVObjHandle UAVObjRegister(uint32_t id, const char* name, const char* metaName, int32_t isMetaobject,
|
||||
int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb);
|
||||
UAVObjHandle UAVObjGetByID(uint32_t id);
|
||||
UAVObjHandle UAVObjGetByName(char* name);
|
||||
uint32_t UAVObjGetID(UAVObjHandle obj);
|
||||
const char* UAVObjGetName(UAVObjHandle obj);
|
||||
uint32_t UAVObjGetNumBytes(UAVObjHandle obj);
|
||||
uint16_t UAVObjGetNumInstances(UAVObjHandle obj);
|
||||
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj);
|
||||
uint16_t UAVObjCreateInstance(UAVObjHandle obj);
|
||||
int32_t UAVObjIsSingleInstance(UAVObjHandle obj);
|
||||
int32_t UAVObjIsMetaobject(UAVObjHandle obj);
|
||||
int32_t UAVObjIsSettings(UAVObjHandle obj);
|
||||
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn);
|
||||
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut);
|
||||
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId);
|
||||
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId);
|
||||
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId);
|
||||
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file);
|
||||
UAVObjHandle UAVObjLoadFromFile(FILEINFO* file);
|
||||
int32_t UAVObjSaveSettings();
|
||||
int32_t UAVObjLoadSettings();
|
||||
int32_t UAVObjDeleteSettings();
|
||||
int32_t UAVObjSaveMetaobjects();
|
||||
int32_t UAVObjLoadMetaobjects();
|
||||
int32_t UAVObjDeleteMetaobjects();
|
||||
int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn);
|
||||
int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut);
|
||||
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);
|
||||
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut);
|
||||
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn);
|
||||
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut);
|
||||
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask);
|
||||
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue);
|
||||
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask);
|
||||
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb);
|
||||
void UAVObjRequestUpdate(UAVObjHandle obj);
|
||||
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId);
|
||||
void UAVObjUpdated(UAVObjHandle obj);
|
||||
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId);
|
||||
void UAVObjIterate(void (*iterator)(UAVObjHandle obj));
|
||||
|
||||
#endif // UAVOBJECTMANAGER_H
|
@ -1,32 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file uavobjectsinit.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Initialize all objects.
|
||||
* This file is automatically updated by the parser.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef UAVOBJECTSINIT_H
|
||||
#define UAVOBJECTSINIT_H
|
||||
|
||||
void UAVObjectsInitializeAll();
|
||||
|
||||
#endif // UAVOBJECTSINIT_H
|
@ -1,70 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file $(NAMELC).h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the $(NAME) object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: $(XMLFILE).
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef $(NAMEUC)_H
|
||||
#define $(NAMEUC)_H
|
||||
|
||||
// Object constants
|
||||
#define $(NAMEUC)_OBJID $(OBJID)U
|
||||
#define $(NAMEUC)_NAME "$(NAME)"
|
||||
#define $(NAMEUC)_METANAME "$(NAME)Meta"
|
||||
#define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST)
|
||||
#define $(NAMEUC)_ISSETTINGS $(ISSETTINGS)
|
||||
#define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data)
|
||||
|
||||
// Object access macros
|
||||
#define $(NAME)Get(dataOut) UAVObjGetData($(NAME)Handle(), dataOut)
|
||||
#define $(NAME)Set(dataIn) UAVObjSetData($(NAME)Handle(), dataIn)
|
||||
#define $(NAME)InstGet(instId, dataOut) UAVObjGetInstanceData($(NAME)Handle(), instId, dataOut)
|
||||
#define $(NAME)InstSet(instId, dataIn) UAVObjSetInstanceData($(NAME)Handle(), instId, dataIn)
|
||||
#define $(NAME)ConnectQueue(queue) UAVObjConnectQueue($(NAME)Handle(), queue, EV_MASK_ALL_UPDATES)
|
||||
#define $(NAME)ConnectCallback(cb) UAVObjConnectCallback($(NAME)Handle(), cb, EV_MASK_ALL_UPDATES)
|
||||
#define $(NAME)CreateInstance() UAVObjCreateInstance($(NAME)Handle())
|
||||
#define $(NAME)RequestUpdate() UAVObjRequestUpdate($(NAME)Handle())
|
||||
#define $(NAME)RequestInstUpdate(instId) UAVObjRequestInstanceUpdate($(NAME)Handle(), instId)
|
||||
#define $(NAME)Updated() UAVObjUpdated($(NAME)Handle())
|
||||
#define $(NAME)InstUpdated(instId) UAVObjUpdated($(NAME)Handle(), instId)
|
||||
#define $(NAME)GetMetadata(dataOut) UAVObjGetMetadata($(NAME)Handle(), dataOut)
|
||||
#define $(NAME)SetMetadata(dataIn) UAVObjSetMetadata($(NAME)Handle(), dataIn)
|
||||
|
||||
// Object data
|
||||
typedef struct {
|
||||
$(DATAFIELDS)
|
||||
} __attribute__((packed)) $(NAME)Data;
|
||||
|
||||
// Field information
|
||||
$(DATAFIELDINFO)
|
||||
|
||||
// Generic interface functions
|
||||
int32_t $(NAME)Initialize();
|
||||
UAVObjHandle $(NAME)Handle();
|
||||
|
||||
#endif // $(NAMEUC)_H
|
@ -1,349 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2007-2009, Troy D. Hanson
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
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 UTLIST_H
|
||||
#define UTLIST_H
|
||||
|
||||
#define UTLIST_VERSION 1.8
|
||||
|
||||
/*
|
||||
* This file contains macros to manipulate singly and doubly-linked lists.
|
||||
*
|
||||
* 1. LL_ macros: singly-linked lists.
|
||||
* 2. DL_ macros: doubly-linked lists.
|
||||
* 3. CDL_ macros: circular doubly-linked lists.
|
||||
*
|
||||
* To use singly-linked lists, your structure must have a "next" pointer.
|
||||
* To use doubly-linked lists, your structure must "prev" and "next" pointers.
|
||||
* Either way, the pointer to the head of the list must be initialized to NULL.
|
||||
*
|
||||
* ----------------.EXAMPLE -------------------------
|
||||
* struct item {
|
||||
* int id;
|
||||
* struct item *prev, *next;
|
||||
* }
|
||||
*
|
||||
* struct item *list = NULL:
|
||||
*
|
||||
* int main() {
|
||||
* struct item *item;
|
||||
* ... allocate and populate item ...
|
||||
* DL_APPEND(list, item);
|
||||
* }
|
||||
* --------------------------------------------------
|
||||
*
|
||||
* For doubly-linked lists, the append and delete macros are O(1)
|
||||
* For singly-linked lists, append and delete are O(n) but prepend is O(1)
|
||||
* The sort macro is O(n log(n)) for all types of single/double/circular lists.
|
||||
*/
|
||||
|
||||
/******************************************************************************
|
||||
* The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort *
|
||||
* Unwieldy variable names used here to avoid shadowing passed-in variables. *
|
||||
*****************************************************************************/
|
||||
#define LL_SORT(list, cmp) \
|
||||
do { \
|
||||
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
|
||||
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
|
||||
if (list) { \
|
||||
_ls_insize = 1; \
|
||||
_ls_looping = 1; \
|
||||
while (_ls_looping) { \
|
||||
_ls_p = list; \
|
||||
_ls_oldhead = list; \
|
||||
list = NULL; \
|
||||
_ls_tail = NULL; \
|
||||
_ls_nmerges = 0; \
|
||||
while (_ls_p) { \
|
||||
_ls_nmerges++; \
|
||||
_ls_q = _ls_p; \
|
||||
_ls_psize = 0; \
|
||||
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
|
||||
_ls_psize++; \
|
||||
_ls_q = _ls_q->next; \
|
||||
if (!_ls_q) break; \
|
||||
} \
|
||||
_ls_qsize = _ls_insize; \
|
||||
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
|
||||
if (_ls_psize == 0) { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
} else if (_ls_qsize == 0 || !_ls_q) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
} else if (cmp(_ls_p,_ls_q) <= 0) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
} else { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
} \
|
||||
if (_ls_tail) { \
|
||||
_ls_tail->next = _ls_e; \
|
||||
} else { \
|
||||
list = _ls_e; \
|
||||
} \
|
||||
_ls_tail = _ls_e; \
|
||||
} \
|
||||
_ls_p = _ls_q; \
|
||||
} \
|
||||
_ls_tail->next = NULL; \
|
||||
if (_ls_nmerges <= 1) { \
|
||||
_ls_looping=0; \
|
||||
} \
|
||||
_ls_insize *= 2; \
|
||||
} \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define DL_SORT(list, cmp) \
|
||||
do { \
|
||||
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
|
||||
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
|
||||
if (list) { \
|
||||
_ls_insize = 1; \
|
||||
_ls_looping = 1; \
|
||||
while (_ls_looping) { \
|
||||
_ls_p = list; \
|
||||
_ls_oldhead = list; \
|
||||
list = NULL; \
|
||||
_ls_tail = NULL; \
|
||||
_ls_nmerges = 0; \
|
||||
while (_ls_p) { \
|
||||
_ls_nmerges++; \
|
||||
_ls_q = _ls_p; \
|
||||
_ls_psize = 0; \
|
||||
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
|
||||
_ls_psize++; \
|
||||
_ls_q = _ls_q->next; \
|
||||
if (!_ls_q) break; \
|
||||
} \
|
||||
_ls_qsize = _ls_insize; \
|
||||
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
|
||||
if (_ls_psize == 0) { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
} else if (_ls_qsize == 0 || !_ls_q) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
} else if (cmp(_ls_p,_ls_q) <= 0) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
} else { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
} \
|
||||
if (_ls_tail) { \
|
||||
_ls_tail->next = _ls_e; \
|
||||
} else { \
|
||||
list = _ls_e; \
|
||||
} \
|
||||
_ls_e->prev = _ls_tail; \
|
||||
_ls_tail = _ls_e; \
|
||||
} \
|
||||
_ls_p = _ls_q; \
|
||||
} \
|
||||
list->prev = _ls_tail; \
|
||||
_ls_tail->next = NULL; \
|
||||
if (_ls_nmerges <= 1) { \
|
||||
_ls_looping=0; \
|
||||
} \
|
||||
_ls_insize *= 2; \
|
||||
} \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define CDL_SORT(list, cmp) \
|
||||
do { \
|
||||
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
|
||||
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
|
||||
if (list) { \
|
||||
_ls_insize = 1; \
|
||||
_ls_looping = 1; \
|
||||
while (_ls_looping) { \
|
||||
_ls_p = list; \
|
||||
_ls_oldhead = list; \
|
||||
list = NULL; \
|
||||
_ls_tail = NULL; \
|
||||
_ls_nmerges = 0; \
|
||||
while (_ls_p) { \
|
||||
_ls_nmerges++; \
|
||||
_ls_q = _ls_p; \
|
||||
_ls_psize = 0; \
|
||||
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
|
||||
_ls_psize++; \
|
||||
_ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \
|
||||
if (!_ls_q) break; \
|
||||
} \
|
||||
_ls_qsize = _ls_insize; \
|
||||
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
|
||||
if (_ls_psize == 0) { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \
|
||||
} else if (_ls_qsize == 0 || !_ls_q) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \
|
||||
} else if (cmp(_ls_p,_ls_q) <= 0) { \
|
||||
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
|
||||
if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \
|
||||
} else { \
|
||||
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
|
||||
if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \
|
||||
} \
|
||||
if (_ls_tail) { \
|
||||
_ls_tail->next = _ls_e; \
|
||||
} else { \
|
||||
list = _ls_e; \
|
||||
} \
|
||||
_ls_e->prev = _ls_tail; \
|
||||
_ls_tail = _ls_e; \
|
||||
} \
|
||||
_ls_p = _ls_q; \
|
||||
} \
|
||||
list->prev = _ls_tail; \
|
||||
_ls_tail->next = list; \
|
||||
if (_ls_nmerges <= 1) { \
|
||||
_ls_looping=0; \
|
||||
} \
|
||||
_ls_insize *= 2; \
|
||||
} \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
/******************************************************************************
|
||||
* singly linked list macros (non-circular) *
|
||||
*****************************************************************************/
|
||||
#define LL_PREPEND(head,add) \
|
||||
do { \
|
||||
(add)->next = head; \
|
||||
head = add; \
|
||||
} while (0)
|
||||
|
||||
#define LL_APPEND(head,add) \
|
||||
do { \
|
||||
__typeof__(head) _tmp; \
|
||||
(add)->next=NULL; \
|
||||
if (head) { \
|
||||
_tmp = head; \
|
||||
while (_tmp->next) { _tmp = _tmp->next; } \
|
||||
_tmp->next=(add); \
|
||||
} else { \
|
||||
(head)=(add); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define LL_DELETE(head,del) \
|
||||
do { \
|
||||
__typeof__(head) _tmp; \
|
||||
if ((head) == (del)) { \
|
||||
(head)=(head)->next; \
|
||||
} else { \
|
||||
_tmp = head; \
|
||||
while (_tmp->next && (_tmp->next != (del))) { \
|
||||
_tmp = _tmp->next; \
|
||||
} \
|
||||
if (_tmp->next) { \
|
||||
_tmp->next = ((del)->next); \
|
||||
} \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define LL_FOREACH(head,el) \
|
||||
for(el=head;el;el=el->next)
|
||||
|
||||
/******************************************************************************
|
||||
* doubly linked list macros (non-circular) *
|
||||
*****************************************************************************/
|
||||
#define DL_PREPEND(head,add) \
|
||||
do { \
|
||||
(add)->next = head; \
|
||||
if (head) { \
|
||||
(add)->prev = (head)->prev; \
|
||||
(head)->prev = (add); \
|
||||
} else { \
|
||||
(add)->prev = (add); \
|
||||
} \
|
||||
(head) = (add); \
|
||||
} while (0)
|
||||
|
||||
#define DL_APPEND(head,add) \
|
||||
do { \
|
||||
if (head) { \
|
||||
(add)->prev = (head)->prev; \
|
||||
(head)->prev->next = (add); \
|
||||
(head)->prev = (add); \
|
||||
(add)->next = NULL; \
|
||||
} else { \
|
||||
(head)=(add); \
|
||||
(head)->prev = (head); \
|
||||
(head)->next = NULL; \
|
||||
} \
|
||||
} while (0);
|
||||
|
||||
#define DL_DELETE(head,del) \
|
||||
do { \
|
||||
if ((del)->prev == (del)) { \
|
||||
(head)=NULL; \
|
||||
} else if ((del)==(head)) { \
|
||||
(del)->next->prev = (del)->prev; \
|
||||
(head) = (del)->next; \
|
||||
} else { \
|
||||
(del)->prev->next = (del)->next; \
|
||||
if ((del)->next) { \
|
||||
(del)->next->prev = (del)->prev; \
|
||||
} else { \
|
||||
(head)->prev = (del)->prev; \
|
||||
} \
|
||||
} \
|
||||
} while (0);
|
||||
|
||||
|
||||
#define DL_FOREACH(head,el) \
|
||||
for(el=head;el;el=el->next)
|
||||
|
||||
/******************************************************************************
|
||||
* circular doubly linked list macros *
|
||||
*****************************************************************************/
|
||||
#define CDL_PREPEND(head,add) \
|
||||
do { \
|
||||
if (head) { \
|
||||
(add)->prev = (head)->prev; \
|
||||
(add)->next = (head); \
|
||||
(head)->prev = (add); \
|
||||
(add)->prev->next = (add); \
|
||||
} else { \
|
||||
(add)->prev = (add); \
|
||||
(add)->next = (add); \
|
||||
} \
|
||||
(head)=(add); \
|
||||
} while (0)
|
||||
|
||||
#define CDL_DELETE(head,del) \
|
||||
do { \
|
||||
if ( ((head)==(del)) && ((head)->next == (head))) { \
|
||||
(head) = 0L; \
|
||||
} else { \
|
||||
(del)->next->prev = (del)->prev; \
|
||||
(del)->prev->next = (del)->next; \
|
||||
if ((del) == (head)) (head)=(del)->next; \
|
||||
} \
|
||||
} while (0);
|
||||
|
||||
#define CDL_FOREACH(head,el) \
|
||||
for(el=head;el;el= (el->next==head ? 0L : el->next))
|
||||
|
||||
|
||||
#endif /* UTLIST_H */
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrolcommand.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ManualControlCommand object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: manualcontrolcommand.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "manualcontrolcommand.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ManualControlCommandInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(MANUALCONTROLCOMMAND_OBJID, MANUALCONTROLCOMMAND_NAME, MANUALCONTROLCOMMAND_METANAME, 0,
|
||||
MANUALCONTROLCOMMAND_ISSINGLEINST, MANUALCONTROLCOMMAND_ISSETTINGS, MANUALCONTROLCOMMAND_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ManualControlCommandData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ManualControlCommandData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 2000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ManualControlCommandHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,132 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file manualcontrolsettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ManualControlSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: manualcontrolsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "manualcontrolsettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ManualControlSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(MANUALCONTROLSETTINGS_OBJID, MANUALCONTROLSETTINGS_NAME, MANUALCONTROLSETTINGS_METANAME, 0,
|
||||
MANUALCONTROLSETTINGS_ISSINGLEINST, MANUALCONTROLSETTINGS_ISSETTINGS, MANUALCONTROLSETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ManualControlSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ManualControlSettingsData));
|
||||
data.InputMode = 0;
|
||||
data.Roll = 0;
|
||||
data.Pitch = 1;
|
||||
data.Yaw = 2;
|
||||
data.Throttle = 3;
|
||||
data.FlightMode = 4;
|
||||
data.ChannelMax[0] = 2000;
|
||||
data.ChannelMax[1] = 2000;
|
||||
data.ChannelMax[2] = 2000;
|
||||
data.ChannelMax[3] = 2000;
|
||||
data.ChannelMax[4] = 2000;
|
||||
data.ChannelMax[5] = 2000;
|
||||
data.ChannelMax[6] = 2000;
|
||||
data.ChannelMax[7] = 2000;
|
||||
data.ChannelNeutral[0] = 1500;
|
||||
data.ChannelNeutral[1] = 1500;
|
||||
data.ChannelNeutral[2] = 1500;
|
||||
data.ChannelNeutral[3] = 1500;
|
||||
data.ChannelNeutral[4] = 1500;
|
||||
data.ChannelNeutral[5] = 1500;
|
||||
data.ChannelNeutral[6] = 1500;
|
||||
data.ChannelNeutral[7] = 1500;
|
||||
data.ChannelMin[0] = 1000;
|
||||
data.ChannelMin[1] = 1000;
|
||||
data.ChannelMin[2] = 1000;
|
||||
data.ChannelMin[3] = 1000;
|
||||
data.ChannelMin[4] = 1000;
|
||||
data.ChannelMin[5] = 1000;
|
||||
data.ChannelMin[6] = 1000;
|
||||
data.ChannelMin[7] = 1000;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ManualControlSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file objectpersistence.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the ObjectPersistence object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: objectpersistence.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "objectpersistence.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t ObjectPersistenceInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(OBJECTPERSISTENCE_OBJID, OBJECTPERSISTENCE_NAME, OBJECTPERSISTENCE_METANAME, 0,
|
||||
OBJECTPERSISTENCE_ISSINGLEINST, OBJECTPERSISTENCE_ISSETTINGS, OBJECTPERSISTENCE_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ObjectPersistenceData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(ObjectPersistenceData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle ObjectPersistenceHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file positionactual.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the PositionActual object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: positionactual.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "positionactual.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t PositionActualInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(POSITIONACTUAL_OBJID, POSITIONACTUAL_NAME, POSITIONACTUAL_METANAME, 0,
|
||||
POSITIONACTUAL_ISSINGLEINST, POSITIONACTUAL_ISSETTINGS, POSITIONACTUAL_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
PositionActualData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(PositionActualData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.loggingUpdatePeriod = 1000;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle PositionActualHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,115 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file stabilizationsettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the StabilizationSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: stabilizationsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "stabilizationsettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t StabilizationSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(STABILIZATIONSETTINGS_OBJID, STABILIZATIONSETTINGS_NAME, STABILIZATIONSETTINGS_METANAME, 0,
|
||||
STABILIZATIONSETTINGS_ISSINGLEINST, STABILIZATIONSETTINGS_ISSETTINGS, STABILIZATIONSETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
StabilizationSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(StabilizationSettingsData));
|
||||
data.UpdatePeriod = 10;
|
||||
data.RollMax = 35;
|
||||
data.PitchMax = 35;
|
||||
data.ThrottleMax = 1;
|
||||
data.PitchKp = 0.04;
|
||||
data.PitchKi = 4e-06;
|
||||
data.PitchKd = 0.01;
|
||||
data.RollKp = 0.02;
|
||||
data.RollKi = 4e-06;
|
||||
data.RollKd = 0.01;
|
||||
data.YawKp = 1;
|
||||
data.YawKi = 0;
|
||||
data.YawKd = 0;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle StabilizationSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemalarms.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemAlarms object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemalarms.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "systemalarms.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t SystemAlarmsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(SYSTEMALARMS_OBJID, SYSTEMALARMS_NAME, SYSTEMALARMS_METANAME, 0,
|
||||
SYSTEMALARMS_ISSINGLEINST, SYSTEMALARMS_ISSETTINGS, SYSTEMALARMS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
SystemAlarmsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(SystemAlarmsData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 4000;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.loggingUpdatePeriod = 1000;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle SystemAlarmsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,103 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemsettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemSettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemsettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "systemsettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t SystemSettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(SYSTEMSETTINGS_OBJID, SYSTEMSETTINGS_NAME, SYSTEMSETTINGS_METANAME, 0,
|
||||
SYSTEMSETTINGS_ISSINGLEINST, SYSTEMSETTINGS_ISSETTINGS, SYSTEMSETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
SystemSettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(SystemSettingsData));
|
||||
data.AirframeType = 0;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle SystemSettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,102 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file systemstats.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the SystemStats object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: systemstats.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "systemstats.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t SystemStatsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(SYSTEMSTATS_OBJID, SYSTEMSTATS_NAME, SYSTEMSTATS_METANAME, 0,
|
||||
SYSTEMSTATS_ISSINGLEINST, SYSTEMSTATS_ISSETTINGS, SYSTEMSTATS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
SystemStatsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(SystemStatsData));
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 0;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.telemetryUpdatePeriod = 1000;
|
||||
metadata.gcsTelemetryAcked = 0;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_MANUAL;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_PERIODIC;
|
||||
metadata.loggingUpdatePeriod = 1000;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle SystemStatsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,103 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file telemetrysettings.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Implementation of the TelemetrySettings object. This file has been
|
||||
* automatically generated by the UAVObjectGenerator.
|
||||
*
|
||||
* @note Object definition file: telemetrysettings.xml.
|
||||
* This is an automatically generated file.
|
||||
* DO NOT modify manually.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "telemetrysettings.h"
|
||||
|
||||
// Private variables
|
||||
static UAVObjHandle handle;
|
||||
|
||||
// Private functions
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId);
|
||||
|
||||
/**
|
||||
* Initialize object.
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t TelemetrySettingsInitialize()
|
||||
{
|
||||
// Register object with the object manager
|
||||
handle = UAVObjRegister(TELEMETRYSETTINGS_OBJID, TELEMETRYSETTINGS_NAME, TELEMETRYSETTINGS_METANAME, 0,
|
||||
TELEMETRYSETTINGS_ISSINGLEINST, TELEMETRYSETTINGS_ISSETTINGS, TELEMETRYSETTINGS_NUMBYTES, &setDefaults);
|
||||
|
||||
// Done
|
||||
if (handle != 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize object fields and metadata with the default values.
|
||||
* If a default value is not specified the object fields
|
||||
* will be initialized to zero.
|
||||
*/
|
||||
static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
TelemetrySettingsData data;
|
||||
UAVObjMetadata metadata;
|
||||
|
||||
// Initialize object fields to their default values
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(TelemetrySettingsData));
|
||||
data.Speed = 2;
|
||||
|
||||
UAVObjSetInstanceData(obj, instId, &data);
|
||||
|
||||
// Initialize object metadata to their default values
|
||||
metadata.access = ACCESS_READWRITE;
|
||||
metadata.gcsAccess = ACCESS_READWRITE;
|
||||
metadata.telemetryAcked = 1;
|
||||
metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.telemetryUpdatePeriod = 0;
|
||||
metadata.gcsTelemetryAcked = 1;
|
||||
metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
metadata.gcsTelemetryUpdatePeriod = 0;
|
||||
metadata.loggingUpdateMode = UPDATEMODE_NEVER;
|
||||
metadata.loggingUpdatePeriod = 0;
|
||||
UAVObjSetMetadata(obj, &metadata);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get object handle
|
||||
*/
|
||||
UAVObjHandle TelemetrySettingsHandle()
|
||||
{
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -1,1475 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file uavobjectmanager.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Object manager library. This library holds a collection of all objects.
|
||||
* It can be used by all modules/libraries to find an object reference.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
|
||||
// Constants
|
||||
|
||||
// Private types
|
||||
|
||||
/**
|
||||
* List of event queues and the eventmask associated with the queue.
|
||||
*/
|
||||
struct ObjectEventListStruct {
|
||||
xQueueHandle queue;
|
||||
UAVObjEventCallback cb;
|
||||
int32_t eventMask;
|
||||
struct ObjectEventListStruct* next;
|
||||
};
|
||||
typedef struct ObjectEventListStruct ObjectEventList;
|
||||
|
||||
/**
|
||||
* List of object instances, holds the actual data structure and instance ID
|
||||
*/
|
||||
struct ObjectInstListStruct {
|
||||
void* data;
|
||||
uint16_t instId;
|
||||
struct ObjectInstListStruct* next;
|
||||
};
|
||||
typedef struct ObjectInstListStruct ObjectInstList;
|
||||
|
||||
/**
|
||||
* List of objects registered in the object manager
|
||||
*/
|
||||
struct ObjectListStruct {
|
||||
uint32_t id; /** The object ID */
|
||||
const char* name; /** The object name */
|
||||
int8_t isMetaobject; /** Set to 1 if this is a metaobject */
|
||||
int8_t isSingleInstance; /** Set to 1 if this object has a single instance */
|
||||
int8_t isSettings; /** Set to 1 if this object is a settings object */
|
||||
uint16_t numBytes; /** Number of data bytes contained in the object (for a single instance) */
|
||||
uint16_t numInstances; /** Number of instances */
|
||||
UAVObjInitializeCallback initCb; /** Object field and metadata initialization callback */
|
||||
struct ObjectListStruct* linkedObj; /** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */
|
||||
ObjectInstList* instances; /** List of object instances, instance 0 always exists */
|
||||
ObjectEventList* events; /** Event queues registered on the object */
|
||||
struct ObjectListStruct* next; /** Needed by linked list library (utlist.h) */
|
||||
};
|
||||
typedef struct ObjectListStruct ObjectList;
|
||||
|
||||
// Private functions
|
||||
static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event);
|
||||
static ObjectInstList* createInstance(ObjectList* obj, uint16_t instId);
|
||||
static ObjectInstList* getInstance(ObjectList* obj, uint16_t instId);
|
||||
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask);
|
||||
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb);
|
||||
static void objectFilename(ObjectList* obj, uint8_t* filename);
|
||||
static void customSPrintf(uint8_t* buffer, uint8_t* format, ...);
|
||||
|
||||
// Private variables
|
||||
static ObjectList* objList;
|
||||
static xSemaphoreHandle mutex;
|
||||
static UAVObjMetadata defMetadata;
|
||||
static UAVObjStats stats;
|
||||
|
||||
/**
|
||||
* Initialize the object manager
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVObjInitialize()
|
||||
{
|
||||
// Initialize variables
|
||||
objList = NULL;
|
||||
memset(&stats, 0, sizeof(UAVObjStats));
|
||||
|
||||
// Create mutex
|
||||
mutex = xSemaphoreCreateRecursiveMutex();
|
||||
if (mutex == NULL)
|
||||
return -1;
|
||||
|
||||
// Initialize default metadata structure (metadata of metaobjects)
|
||||
defMetadata.access = ACCESS_READWRITE;
|
||||
defMetadata.gcsAccess = ACCESS_READWRITE;
|
||||
defMetadata.telemetryAcked = 1;
|
||||
defMetadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
defMetadata.telemetryUpdatePeriod = 0;
|
||||
defMetadata.gcsTelemetryAcked = 1;
|
||||
defMetadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
defMetadata.gcsTelemetryUpdatePeriod = 0;
|
||||
defMetadata.loggingUpdateMode = UPDATEMODE_ONCHANGE;
|
||||
defMetadata.loggingUpdatePeriod = 0;
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the statistics counters
|
||||
* @param[out] statsOut The statistics counters will be copied there
|
||||
*/
|
||||
void UAVObjGetStats(UAVObjStats* statsOut)
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
memcpy(statsOut, &stats, sizeof(UAVObjStats));
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Clear the statistics counters
|
||||
*/
|
||||
void UAVObjClearStats()
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
memset(&stats, 0, sizeof(UAVObjStats));
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Register and new object in the object manager.
|
||||
* \param[in] id Unique object ID
|
||||
* \param[in] name Object name
|
||||
* \param[in] nameName Metaobject name
|
||||
* \param[in] isMetaobject Is this a metaobject (1:true, 0:false)
|
||||
* \param[in] isSingleInstance Is this a single instance or multi-instance object
|
||||
* \param[in] isSettings Is this a settings object
|
||||
* \param[in] numBytes Number of bytes of object data (for one instance)
|
||||
* \param[in] initCb Default field and metadata initialization function
|
||||
* \return Object handle, or NULL if failure.
|
||||
* \return
|
||||
*/
|
||||
UAVObjHandle UAVObjRegister(uint32_t id, const char* name, const char* metaName, int32_t isMetaobject,
|
||||
int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
ObjectList* metaObj;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Check that the object is not already registered
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
if (objEntry->id == id)
|
||||
{
|
||||
// Already registered, ignore
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
// Create and append entry
|
||||
objEntry = (ObjectList*)pvPortMalloc(sizeof(ObjectList));
|
||||
if (objEntry == NULL)
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
objEntry->id = id;
|
||||
objEntry->name = name;
|
||||
objEntry->isMetaobject = (int8_t)isMetaobject;
|
||||
objEntry->isSingleInstance = (int8_t)isSingleInstance;
|
||||
objEntry->isSettings = (int8_t)isSettings;
|
||||
objEntry->numBytes = numBytes;
|
||||
objEntry->events = NULL;
|
||||
objEntry->numInstances = 0;
|
||||
objEntry->initCb = initCb;
|
||||
objEntry->instances = NULL;
|
||||
objEntry->linkedObj = NULL; // will be set later
|
||||
LL_APPEND(objList, objEntry);
|
||||
|
||||
// Create instance zero
|
||||
instEntry = createInstance(objEntry, 0);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Create metaobject and update linkedObj
|
||||
if (isMetaobject)
|
||||
{
|
||||
objEntry->linkedObj = NULL; // will be set later
|
||||
}
|
||||
else
|
||||
{
|
||||
// Create metaobject
|
||||
metaObj = (ObjectList*)UAVObjRegister(id+1, metaName, NULL, 1, 1, 0, sizeof(UAVObjMetadata), NULL);
|
||||
// Link two objects
|
||||
objEntry->linkedObj = metaObj;
|
||||
metaObj->linkedObj = objEntry;
|
||||
}
|
||||
|
||||
// Initialize object fields and metadata to default values
|
||||
if ( objEntry->initCb != NULL )
|
||||
{
|
||||
objEntry->initCb((UAVObjHandle)objEntry, 0);
|
||||
}
|
||||
|
||||
// Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object)
|
||||
if ( !objEntry->isMetaobject )
|
||||
{
|
||||
UAVObjLoad( (UAVObjHandle)objEntry->linkedObj, 0 );
|
||||
}
|
||||
|
||||
// If this is a settings object, attempt to load from SD card
|
||||
if ( objEntry->isSettings )
|
||||
{
|
||||
UAVObjLoad( (UAVObjHandle)objEntry, 0 );
|
||||
}
|
||||
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return (UAVObjHandle)objEntry;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve an object from the list given its id
|
||||
* \param[in] The object ID
|
||||
* \return The object or NULL if not found.
|
||||
*/
|
||||
UAVObjHandle UAVObjGetByID(uint32_t id)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Look for object
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
if (objEntry->id == id)
|
||||
{
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
// Done, object found
|
||||
return (UAVObjHandle)objEntry;
|
||||
}
|
||||
}
|
||||
|
||||
// Object not found, release lock and return error
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieve an object from the list given its name
|
||||
* \param[in] name The name of the object
|
||||
* \return The object or NULL if not found.
|
||||
*/
|
||||
UAVObjHandle UAVObjGetByName(char* name)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Look for object
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
if (objEntry->name != NULL && strcmp(objEntry->name, name) == 0)
|
||||
{
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
// Done, object found
|
||||
return (UAVObjHandle)objEntry;
|
||||
}
|
||||
}
|
||||
|
||||
// Object not found, release lock and return error
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the object's ID
|
||||
* \param[in] obj The object handle
|
||||
* \return The object ID
|
||||
*/
|
||||
uint32_t UAVObjGetID(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->id;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the object's name
|
||||
* \param[in] obj The object handle
|
||||
* \return The object's name
|
||||
*/
|
||||
const char* UAVObjGetName(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the number of bytes of the object's data (for one instance)
|
||||
* \param[in] obj The object handle
|
||||
* \return The number of bytes
|
||||
*/
|
||||
uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->numBytes;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the object this object is linked to. For regular objects, the linked object
|
||||
* is the metaobject. For metaobjects the linked object is the parent object.
|
||||
* This function is normally only needed by the telemetry module.
|
||||
* \param[in] obj The object handle
|
||||
* \return The object linked object handle
|
||||
*/
|
||||
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
|
||||
{
|
||||
return (UAVObjHandle)(((ObjectList*)obj)->linkedObj);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the number of instances contained in the object.
|
||||
* \param[in] obj The object handle
|
||||
* \return The number of instances
|
||||
*/
|
||||
uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
|
||||
{
|
||||
uint32_t numInstances;
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
numInstances = ((ObjectList*)obj)->numInstances;
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return numInstances;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new instance in the object.
|
||||
* \param[in] obj The object handle
|
||||
* \return The instance ID or 0 if an error
|
||||
*/
|
||||
uint16_t UAVObjCreateInstance(UAVObjHandle obj)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Create new instance
|
||||
objEntry = (ObjectList*)obj;
|
||||
instEntry = createInstance(objEntry, objEntry->numInstances);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Initialize instance data
|
||||
if ( objEntry->initCb != NULL )
|
||||
{
|
||||
objEntry->initCb(obj, instEntry->instId);
|
||||
}
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return instEntry->instId;
|
||||
}
|
||||
|
||||
/**
|
||||
* Does this object contains a single instance or multiple instances?
|
||||
* \param[in] obj The object handle
|
||||
* \return True (1) if this is a single instance object
|
||||
*/
|
||||
int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->isSingleInstance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Is this a metaobject?
|
||||
* \param[in] obj The object handle
|
||||
* \return True (1) if this is metaobject
|
||||
*/
|
||||
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->isMetaobject;
|
||||
}
|
||||
|
||||
/**
|
||||
* Is this a settings object?
|
||||
* \param[in] obj The object handle
|
||||
* \return True (1) if this is a settings object
|
||||
*/
|
||||
int32_t UAVObjIsSettings(UAVObjHandle obj)
|
||||
{
|
||||
return ((ObjectList*)obj)->isSettings;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unpack an object from a byte array
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId The instance ID
|
||||
* \param[in] dataIn The byte array
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast handle to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get the instance
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
|
||||
// If the instance does not exist create it and any other instances before it
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
instEntry = createInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
// Error, unlock and return
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Set the data
|
||||
memcpy(instEntry->data, dataIn, objEntry->numBytes);
|
||||
|
||||
// Fire event
|
||||
sendEvent(objEntry, instId, EV_UNPACKED);
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Pack an object to a byte array
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId The instance ID
|
||||
* \param[out] dataOut The byte array
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast handle to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get the instance
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
// Error, unlock and return
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Pack data
|
||||
memcpy(dataOut, instEntry->data, objEntry->numBytes);
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save the data of the specified object instance to the file system (SD card).
|
||||
* The object will be appended and the file will not be closed.
|
||||
* The object data can be restored using the UAVObjLoad function.
|
||||
* @param[in] obj The object handle.
|
||||
* @param[in] instId The instance ID
|
||||
* @param[in] file File to append to
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file)
|
||||
{
|
||||
uint32_t bytesWritten;
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Check for file system availability
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get the instance information
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Write the object ID
|
||||
PIOS_FWRITE(file,&objEntry->id,sizeof(objEntry->id),&bytesWritten);
|
||||
|
||||
// Write the instance ID
|
||||
if (!objEntry->isSingleInstance)
|
||||
{
|
||||
PIOS_FWRITE(file,&instEntry->instId,sizeof(instEntry->instId),&bytesWritten);
|
||||
}
|
||||
|
||||
// Write the data and check that the write was successful
|
||||
PIOS_FWRITE(file,instEntry->data,objEntry->numBytes,&bytesWritten);
|
||||
if ( bytesWritten != objEntry->numBytes )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save the data of the specified object to the file system (SD card).
|
||||
* If the object contains multiple instances, all of them will be saved.
|
||||
* A new file with the name of the object will be created.
|
||||
* The object data can be restored using the UAVObjLoad function.
|
||||
* @param[in] obj The object handle.
|
||||
* @param[in] instId The instance ID
|
||||
* @param[in] file File to append to
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
FILEINFO file;
|
||||
ObjectList* objEntry;
|
||||
uint8_t filename[14];
|
||||
|
||||
// Check for file system availability
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get filename
|
||||
objectFilename(objEntry, filename);
|
||||
|
||||
// Open file
|
||||
if ( PIOS_FOPEN_WRITE(filename,file) )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Append object
|
||||
if ( UAVObjSaveToFile(obj, instId, &file) == -1 )
|
||||
{
|
||||
PIOS_FCLOSE(file);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Done, close file and unlock
|
||||
PIOS_FCLOSE(file);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Load an object from the file system (SD card).
|
||||
* @param[in] file File to read from
|
||||
* @return The handle of the object loaded or NULL if a failure
|
||||
*/
|
||||
UAVObjHandle UAVObjLoadFromFile(FILEINFO* file)
|
||||
{
|
||||
uint32_t bytesRead;
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
uint32_t objId;
|
||||
uint16_t instId;
|
||||
UAVObjHandle obj;
|
||||
|
||||
// Check for file system availability
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Read the object ID
|
||||
if ( PIOS_FREAD(file,&objId,sizeof(objId),&bytesRead) )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Get the object
|
||||
obj = UAVObjGetByID(objId);
|
||||
if ( obj == 0 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get the instance ID
|
||||
instId = 0;
|
||||
if ( !objEntry->isSingleInstance )
|
||||
{
|
||||
if ( PIOS_FREAD(file,&instId,sizeof(instId),&bytesRead) )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
// Get the instance information
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
|
||||
// If the instance does not exist create it and any other instances before it
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
instEntry = createInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
// Error, unlock and return
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
// Read the instance data
|
||||
if ( PIOS_FREAD(file,instEntry->data,objEntry->numBytes,&bytesRead) )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Fire event
|
||||
sendEvent(objEntry, instId, EV_UNPACKED);
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return obj;
|
||||
}
|
||||
|
||||
/**
|
||||
* Load an object from the file system (SD card).
|
||||
* A file with the name of the object will be opened.
|
||||
* The object data can be saved using the UAVObjSave function.
|
||||
* @param[in] obj The object handle.
|
||||
* @param[in] instId The object instance
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
FILEINFO file;
|
||||
ObjectList* objEntry;
|
||||
UAVObjHandle loadedObj;
|
||||
ObjectList* loadedObjEntry;
|
||||
uint8_t filename[14];
|
||||
|
||||
// Check for file system availability
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get filename
|
||||
objectFilename(objEntry, filename);
|
||||
|
||||
// Open file
|
||||
if ( PIOS_FOPEN_READ(filename,file) )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Load object
|
||||
loadedObj = UAVObjLoadFromFile(&file);
|
||||
if (loadedObj == 0)
|
||||
{
|
||||
PIOS_FCLOSE(file);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Check that the IDs match
|
||||
loadedObjEntry = (ObjectList*)loadedObj;
|
||||
if ( loadedObjEntry->id != objEntry->id )
|
||||
{
|
||||
PIOS_FCLOSE(file);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Done, close file and unlock
|
||||
PIOS_FCLOSE(file);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Delete an object from the file system (SD card).
|
||||
* @param[in] obj The object handle.
|
||||
* @param[in] instId The object instance
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
uint8_t filename[14];
|
||||
|
||||
// Check for file system availability
|
||||
if ( POIS_SDCARD_IsMounted() == 0 )
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get filename
|
||||
objectFilename(objEntry, filename);
|
||||
|
||||
// Delete file
|
||||
PIOS_FUNLINK(filename);
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save all settings objects to the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSaveSettings()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Save all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isSettings )
|
||||
{
|
||||
// Save object
|
||||
if ( UAVObjSave( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Load all settings objects from the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjLoadSettings()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Load all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isSettings )
|
||||
{
|
||||
// Load object
|
||||
if ( UAVObjLoad( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Delete all settings objects from the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjDeleteSettings()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Save all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isSettings )
|
||||
{
|
||||
// Save object
|
||||
if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Save all metaobjects to the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSaveMetaobjects()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Save all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isMetaobject )
|
||||
{
|
||||
// Save object
|
||||
if ( UAVObjSave( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Load all metaobjects from the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjLoadMetaobjects()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Load all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isMetaobject )
|
||||
{
|
||||
// Load object
|
||||
if ( UAVObjLoad( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Delete all metaobjects from the SD card.
|
||||
* @return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjDeleteMetaobjects()
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Load all settings objects
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
// Check if this is a settings object
|
||||
if ( objEntry->isMetaobject )
|
||||
{
|
||||
// Load object
|
||||
if ( UAVObjDelete( (UAVObjHandle)objEntry, 0 ) == -1 )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the object data
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] dataIn The object's data structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn)
|
||||
{
|
||||
return UAVObjSetInstanceData(obj, 0, dataIn);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the object data
|
||||
* \param[in] obj The object handle
|
||||
* \param[out] dataOut The object's data structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut)
|
||||
{
|
||||
return UAVObjGetInstanceData(obj, 0, dataOut);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the data of a specific object instance
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId The object instance ID
|
||||
* \param[in] dataIn The object's data structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
UAVObjMetadata* mdata;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object info
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Check access level
|
||||
if ( !objEntry->isMetaobject )
|
||||
{
|
||||
mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances->data);
|
||||
if ( mdata->access == ACCESS_READONLY )
|
||||
{
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// Get instance information
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
// Error, unlock and return
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set data
|
||||
memcpy(instEntry->data, dataIn, objEntry->numBytes);
|
||||
|
||||
// Fire event
|
||||
sendEvent(objEntry, instId, EV_UPDATED);
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the data of a specific object instance
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId The object instance ID
|
||||
* \param[out] dataOut The object's data structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Cast to object info
|
||||
objEntry = (ObjectList*)obj;
|
||||
|
||||
// Get instance information
|
||||
instEntry = getInstance(objEntry, instId);
|
||||
if ( instEntry == NULL )
|
||||
{
|
||||
// Error, unlock and return
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set data
|
||||
memcpy(dataOut, instEntry->data, objEntry->numBytes);
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the object metadata
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] dataIn The object's metadata structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Set metadata (metadata of metaobjects can not be modified)
|
||||
objEntry = (ObjectList*)obj;
|
||||
if (!objEntry->isMetaobject)
|
||||
{
|
||||
UAVObjSetData((UAVObjHandle)objEntry->linkedObj, dataIn);
|
||||
}
|
||||
else
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the object metadata
|
||||
* \param[in] obj The object handle
|
||||
* \param[out] dataOut The object's metadata structure
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut)
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Get metadata
|
||||
objEntry = (ObjectList*)obj;
|
||||
if (objEntry->isMetaobject)
|
||||
{
|
||||
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
|
||||
}
|
||||
else
|
||||
{
|
||||
UAVObjGetData((UAVObjHandle)objEntry->linkedObj, dataOut);
|
||||
}
|
||||
|
||||
// Unlock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
|
||||
* All events matching the event mask will be pushed to the event queue.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] queue The event queue
|
||||
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, int32_t eventMask)
|
||||
{
|
||||
int32_t res;
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
res = connectObj(obj, queue, 0, eventMask);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect an event queue from the object.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] queue The event queue
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
|
||||
{
|
||||
int32_t res;
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
res = disconnectObj(obj, queue, 0);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* Connect an event callback to the object, if the callback is already connected then the event mask is only updated.
|
||||
* The supplied callback will be invoked on all events matching the event mask.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] cb The event callback
|
||||
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, int32_t eventMask)
|
||||
{
|
||||
int32_t res;
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
res = connectObj(obj, 0, cb, eventMask);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect an event callback from the object.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] cb The event callback
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
|
||||
{
|
||||
int32_t res;
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
res = disconnectObj(obj, 0, cb);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
|
||||
* will be generated as soon as the object is updated.
|
||||
* \param[in] obj The object handle
|
||||
*/
|
||||
void UAVObjRequestUpdate(UAVObjHandle obj)
|
||||
{
|
||||
UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES);
|
||||
}
|
||||
|
||||
/**
|
||||
* Request an update of the object's data from the GCS. The call will not wait for the response, a EV_UPDATED event
|
||||
* will be generated as soon as the object is updated.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId Object instance ID to update
|
||||
*/
|
||||
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
sendEvent((ObjectList*)obj, instId, EV_UPDATE_REQ);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
|
||||
* \param[in] obj The object handle
|
||||
*/
|
||||
void UAVObjUpdated(UAVObjHandle obj)
|
||||
{
|
||||
UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] instId The object instance ID
|
||||
*/
|
||||
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
sendEvent((ObjectList*)obj, instId, EV_UPDATED_MANUAL);
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Iterate through all objects in the list.
|
||||
* \param iterator This function will be called once for each object,
|
||||
* the object will be passed as a parameter
|
||||
*/
|
||||
void UAVObjIterate(void (*iterator)(UAVObjHandle obj))
|
||||
{
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Get lock
|
||||
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
|
||||
|
||||
// Iterate through the list and invoke iterator for each object
|
||||
LL_FOREACH(objList, objEntry)
|
||||
{
|
||||
(*iterator)((UAVObjHandle)objEntry);
|
||||
}
|
||||
|
||||
// Release lock
|
||||
xSemaphoreGiveRecursive(mutex);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send an event to all event queues registered on the object.
|
||||
*/
|
||||
static int32_t sendEvent(ObjectList* obj, uint16_t instId, UAVObjEventType event)
|
||||
{
|
||||
ObjectEventList* eventEntry;
|
||||
UAVObjEvent msg;
|
||||
|
||||
// Setup event
|
||||
msg.obj = (UAVObjHandle)obj;
|
||||
msg.event = event;
|
||||
msg.instId = instId;
|
||||
|
||||
// Go through each object and push the event message in the queue (if event is activated for the queue)
|
||||
LL_FOREACH(obj->events, eventEntry)
|
||||
{
|
||||
if ( eventEntry->eventMask == 0 || (eventEntry->eventMask & event) != 0 )
|
||||
{
|
||||
// Send to queue if a valid queue is registered
|
||||
if (eventEntry->queue != 0)
|
||||
{
|
||||
if ( xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE ) // will not block
|
||||
{
|
||||
++stats.eventErrors;
|
||||
}
|
||||
}
|
||||
// Invoke callback (from event task) if a valid one is registered
|
||||
if (eventEntry->cb != 0)
|
||||
{
|
||||
if ( EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE ) // invoke callback from the event task, will not block
|
||||
{
|
||||
++stats.eventErrors;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new object instance, return the instance info or NULL if failure.
|
||||
*/
|
||||
static ObjectInstList* createInstance(ObjectList* obj, uint16_t instId)
|
||||
{
|
||||
ObjectInstList* instEntry;
|
||||
int32_t n;
|
||||
|
||||
// For single instance objects, only instance zero is allowed
|
||||
if (obj->isSingleInstance && instId != 0)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Make sure that the instance ID is within limits
|
||||
if (instId >= UAVOBJ_MAX_INSTANCES)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Check if the instance already exists
|
||||
if ( getInstance(obj, instId) != NULL )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
// Create any missing instances (all instance IDs must be sequential)
|
||||
for (n = obj->numInstances; n < instId; ++n)
|
||||
{
|
||||
if ( createInstance(obj, n) == NULL )
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
// Create the actual instance
|
||||
instEntry = (ObjectInstList*)pvPortMalloc(sizeof(ObjectInstList));
|
||||
if (instEntry == NULL) return NULL;
|
||||
instEntry->data = pvPortMalloc(obj->numBytes);
|
||||
if (instEntry->data == NULL) return NULL;
|
||||
memset(instEntry->data, 0, obj->numBytes);
|
||||
instEntry->instId = instId;
|
||||
LL_APPEND(obj->instances, instEntry);
|
||||
++obj->numInstances;
|
||||
|
||||
// Fire event
|
||||
UAVObjInstanceUpdated((UAVObjHandle)obj, instId);
|
||||
|
||||
// Done
|
||||
return instEntry;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the instance information or NULL if the instance does not exist
|
||||
*/
|
||||
static ObjectInstList* getInstance(ObjectList* obj, uint16_t instId)
|
||||
{
|
||||
ObjectInstList* instEntry;
|
||||
|
||||
// Look for specified instance ID
|
||||
LL_FOREACH(obj->instances, instEntry)
|
||||
{
|
||||
if (instEntry->instId == instId)
|
||||
{
|
||||
return instEntry;
|
||||
}
|
||||
}
|
||||
// If this point is reached then instance id was not found
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/**
|
||||
* Connect an event queue to the object, if the queue is already connected then the event mask is only updated.
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] queue The event queue
|
||||
* \param[in] cb The event callback
|
||||
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb, int32_t eventMask)
|
||||
{
|
||||
ObjectEventList* eventEntry;
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Check that the queue is not already connected, if it is simply update event mask
|
||||
objEntry = (ObjectList*)obj;
|
||||
LL_FOREACH(objEntry->events, eventEntry)
|
||||
{
|
||||
if ( eventEntry->queue == queue && eventEntry->cb == cb )
|
||||
{
|
||||
// Already connected, update event mask and return
|
||||
eventEntry->eventMask = eventMask;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Add queue to list
|
||||
eventEntry = (ObjectEventList*)pvPortMalloc(sizeof(ObjectEventList));
|
||||
if (eventEntry == NULL)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
eventEntry->queue = queue;
|
||||
eventEntry->cb = cb;
|
||||
eventEntry->eventMask = eventMask;
|
||||
LL_APPEND(objEntry->events, eventEntry);
|
||||
|
||||
// Done
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect an event queue from the object
|
||||
* \param[in] obj The object handle
|
||||
* \param[in] queue The event queue
|
||||
* \param[in] cb The event callback
|
||||
* \return 0 if success or -1 if failure
|
||||
*/
|
||||
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue, UAVObjEventCallback cb)
|
||||
{
|
||||
ObjectEventList* eventEntry;
|
||||
ObjectList* objEntry;
|
||||
|
||||
// Find queue and remove it
|
||||
objEntry = (ObjectList*)obj;
|
||||
LL_FOREACH(objEntry->events, eventEntry)
|
||||
{
|
||||
if ( ( eventEntry->queue == queue && eventEntry->cb == cb ) )
|
||||
{
|
||||
LL_DELETE(objEntry->events, eventEntry);
|
||||
vPortFree(eventEntry);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// If this point is reached the queue was not found
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrapper for the sprintf function
|
||||
*/
|
||||
static void customSPrintf(uint8_t* buffer, uint8_t* format, ...)
|
||||
{
|
||||
va_list args;
|
||||
va_start(args, format);
|
||||
vsprintf((char *)buffer, (char *)format, args);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get an 8 character (plus extension) filename for the object.
|
||||
*/
|
||||
static void objectFilename(ObjectList* obj, uint8_t* filename)
|
||||
{
|
||||
customSPrintf(filename, (uint8_t*)"%X.obj", obj->id);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue
Block a user