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

Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2

This commit is contained in:
px4dev 2013-05-22 21:39:30 +02:00
parent e67022f874 78d29045c4
commit 437d9e4180
364 changed files with 23971 additions and 121563 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
.gitignore vendored
View File

@ -1,62 +1,26 @@
.built
.context
*.context
*.bdat
*.pdat
.depend
.updated
.config
.config-e
.version
.project
.cproject
apps/builtin/builtin_list.h
apps/builtin/builtin_proto.h
Make.dep
*.pyc
*.o
*.a
*.d
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
nuttx/setenv.sh
nuttx/arch/arm/include/board
nuttx/arch/arm/include/chip
nuttx/arch/arm/src/board
nuttx/arch/arm/src/chip
nuttx/include/apps
nuttx/include/arch
nuttx/include/math.h
nuttx/include/nuttx/config.h
nuttx/include/nuttx/version.h
nuttx/tools/mkconfig
nuttx/tools/mkconfig.exe
nuttx/tools/mkversion
nuttx/tools/mkversion.exe
nuttx/nuttx
nuttx/System.map
nuttx/nuttx.bin
nuttx/nuttx.hex
.configured
.settings
Firmware.sublime-workspace
.DS_Store
cscope.out
.configX-e
nuttx-export.zip
.~lock.*
dot.gdbinit
mavlink/include/mavlink/v0.9/
.*.swp
.swp
core
.gdbinit
mkdeps
Archives
Build
!ROMFS/*/*.d
!ROMFS/*/*/*.d
!ROMFS/*/*/*/*.d
*.dSYM
*.o
*.pyc
*~
.*.swp
.context
.cproject
.DS_Store
.gdbinit
.project
.settings
.swp
.~lock.*
Archives/*
Build/*
core
cscope.out
dot.gdbinit
Firmware.sublime-workspace
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/

16650
Documentation/control_flow.graffle Normal file
View File

File diff suppressed because it is too large Load Diff

18
Makefile
View File

@ -1,5 +1,5 @@
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -95,9 +95,14 @@ all: $(STAGED_FIRMWARES)
#
# Copy FIRMWARES into the image directory.
#
# XXX copying the .bin files is a hack to work around the PX4IO uploader
# not supporting .px4 files, and it should be deprecated onced that
# is taken care of.
#
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@echo %% Copying $@
$(Q) $(COPY) $< $@
$(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@)
#
# Generate FIRMWARES.
@ -140,7 +145,7 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
endif
$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@))
$(ARCHIVE_DIR)%.export: configuration = $(if $(findstring px4io,$(board)),io,nsh)
$(ARCHIVE_DIR)%.export: configuration = $(if $(filter px4io px4iov2,$(board)),io,nsh)
$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
@echo %% Configuring NuttX for $(board)/$(configuration)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
@ -159,11 +164,11 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
.PHONY: clean
clean:
$(Q) $(RMDIR) $(BUILD_DIR)*.build
$(Q) $(REMOVE) -f $(IMAGE_DIR)*.px4
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) -f $(ARCHIVE_DIR)*.export
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
#
@ -196,6 +201,11 @@ help:
@echo " distclean"
@echo " Remove all compilation products, including NuttX RTOS archives."
@echo ""
@echo " upload"
@echo " When exactly one config is being built, add this target to upload the"
@echo " firmware to the board when the build is complete. Not supported for"
@echo " all configurations."
@echo ""
@echo " Common options:"
@echo " ---------------"
@echo ""

6
ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
View File

@ -20,10 +20,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

107
ROMFS/px4fmu_common/init.d/rc.IO_QUAD Normal file
View File

@ -0,0 +1,107 @@
#!nsh
# Disable USB and autostart
set USB no
set MODE quad
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
fi
fi
fi
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
multirotor_att_control start
#
# Start logging
#
#sdlog start -s 4
#
# Start system state
#
if blinkm start
then
echo "using BlinkM for state indication"
blinkm systemstate
else
echo "no BlinkM found, OK."
fi

6
ROMFS/px4fmu_common/init.d/rc.PX4IO
View File

@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

8
ROMFS/px4fmu_common/init.d/rc.PX4IOAR
View File

@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Init the parameter storage
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -17,10 +17,10 @@ hil mode_pwm
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

8
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -7,6 +7,14 @@
# Start sensor drivers here.
#
#
# Check for UORB
#
if uorb start
then
echo "uORB started"
fi
ms5611 start
adc start

53
ROMFS/px4fmu_common/mixers/FMU_CCPM.mix Normal file
View File

@ -0,0 +1,53 @@
Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
==================================================
Output 0 - Rear Servo Mixer
----------------
Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
M: 2
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
Output 1 - Left Servo Mixer
-----------------
Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 8660 8660 0 -10000 10000
Output 2 - Right Servo Mixer
----------------
Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 -8660 -8660 0 -10000 10000
Output 3 - Tail Servo Mixer
----------------
Tail Servo = Yaw (control index = 2)
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Output 4 - Motor speed mixer
-----------------
This would be the motor speed control output from governor power demand- not sure what index to use here?
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 0 20000 -10000 -10000 10000

6
ROMFS/px4fmu_common/mixers/FMU_quad_w.mix Normal file
View File

@ -0,0 +1,6 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%.
R: 4w 10000 10000 10000 0

10
apps/.gitignore vendored Normal file
View File

@ -0,0 +1,10 @@
*.a
*.bdat
*.pdat
.built
.config
.depend
.updated
builtin/builtin_list.h
builtin/builtin_proto.h
Make.dep

71
makefiles/README.txt Normal file
View File

@ -0,0 +1,71 @@
PX4 Build System
================
The files in this directory implement the PX4 runtime firmware build system
and configuration for the standard PX4 boards and software, in conjunction
with Makefile in the parent directory.
../Makefile
Top-level makefile for the PX4 build system. This makefile supports
building NuttX archives, as well as supervising the building of all
of the defined PX4 firmware configurations.
Try 'make help' in the parent directory for documentation.
firmware.mk
Manages the build for one specific firmware configuration.
See the comments at the top of this file for detailed documentation.
Builds modules, builtin command lists and the ROMFS (if configured).
This is the makefile directly used by external build systems; it can
be configured to compile modules both inside and outside the PX4
source tree. When used in this mode, at least BOARD, MODULES and
CONFIG_FILE must be set.
module.mk
Called by firmware.mk to build individual modules.
See the comments at the top of this file for detailed documentation.
Not normally used other than by firmware.mk.
nuttx.mk
Called by ../Makefile to build or download the NuttX archives.
upload.mk
Called by ../Makefile to upload files to a target board. Can be used
by external build systems as well.
setup.mk
Provides common path and tool definitions. Implements host system-specific
compatibility hacks.
board_<boardname>.mk
Board-specific configuration for <boardname>. Typically sets CONFIG_ARCH
and then includes the toolchain definition for the board.
config_<boardname>_<configname>.mk
Parameters for a specific configuration on a specific board.
The board name is derived from the filename. Sets MODULES to select
source modules to be included in the configuration, may also set
ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module
commands (e.g. from NuttX)
toolchain_<toolchainname>.mk
Provides macros used to compile and link source files.
Accepts EXTRADEFINES to add additional pre-processor symbol definitions,
EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass
additional flags to the C compiler, C++ compiler, assembler and linker
respectively.
Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK
macros that are used elsewhere in the build system.

16
makefiles/config_px4fmu_default.mk
View File

@ -14,6 +14,7 @@ MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu
@ -29,6 +30,9 @@ MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott_telemetry
MODULES += drivers/blinkm
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/ets_airspeed
MODULES += modules/sensors
#
@ -77,15 +81,19 @@ MODULES += modules/multirotor_pos_control
MODULES += modules/sdlog
#
# Libraries
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/mathlib
MODULES += modules/mathlib/CMSIS
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += modules/mathlib/CMSIS
#
# Demo apps
#
@ -102,6 +110,10 @@ MODULES += modules/uORB
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
MODULES += examples/fixedwing_control
#
# Transitional support - add commands from the NuttX export archive.
#

8
makefiles/config_px4fmuv2_default.mk
View File

@ -74,15 +74,19 @@ MODULES += modules/multirotor_pos_control
MODULES += modules/sdlog
#
# Libraries
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/mathlib
MODULES += modules/mathlib/CMSIS
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += modules/mathlib/CMSIS
#
# Demo apps
#

111
makefiles/firmware.mk
View File

@ -180,20 +180,8 @@ EXTRA_CLEANS =
# Modules
################################################################################
#
# We don't actually know what a module is called; all we have is a path fragment
# that we can search for, and where we expect to find a module.mk file.
#
# As such, we replicate the successfully-found path inside WORK_DIR for the
# module's build products in order to keep modules separated from each other.
#
# XXX If this becomes unwieldy or breaks for other reasons, we will need to
# move to allocating directory names and keeping tabs on makefiles via
# the directory name. That will involve arithmetic (it'd probably be time
# for GMSL).
# where to look for modules
MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
# sort and unique the modules list
MODULES := $(sort $(MODULES))
@ -201,9 +189,9 @@ MODULES := $(sort $(MODULES))
# locate the first instance of a module by full path or by looking on the
# module search path
define MODULE_SEARCH
$(abspath $(firstword $(wildcard $(1)/module.mk) \
$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
MISSING_$1))
$(firstword $(abspath $(wildcard $(1)/module.mk)) \
$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
MISSING_$1)
endef
# make a list of module makefiles and check that we found them all
@ -223,12 +211,15 @@ MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module
.PHONY: $(MODULE_OBJS)
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
$(MODULE_OBJS): workdir = $(@D)
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
MODULE_WORK_DIR=$(dir $@) \
-C $(workdir) \
MODULE_WORK_DIR=$(workdir) \
MODULE_OBJ=$@ \
MODULE_MK=$(mkfile) \
MODULE_NAME=$(lastword $(subst /, ,$(@D))) \
MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
module
# make a list of phony clean targets for modules
@ -245,6 +236,66 @@ $(MODULE_CLEANS):
MODULE_MK=$(mkfile) \
clean
################################################################################
# Libraries
################################################################################
# where to look for libraries
LIBRARY_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
# sort and unique the library list
LIBRARIES := $(sort $(LIBRARIES))
# locate the first instance of a library by full path or by looking on the
# library search path
define LIBRARY_SEARCH
$(firstword $(abspath $(wildcard $(1)/library.mk)) \
$(abspath $(foreach search_dir,$(LIBRARY_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/library.mk))) \
MISSING_$1)
endef
# make a list of library makefiles and check that we found them all
LIBRARY_MKFILES := $(foreach library,$(LIBRARIES),$(call LIBRARY_SEARCH,$(library)))
MISSING_LIBRARIES := $(subst MISSING_,,$(filter MISSING_%,$(LIBRARY_MKFILES)))
ifneq ($(MISSING_LIBRARIES),)
$(error Can't find library(s): $(MISSING_LIBRARIES))
endif
# Make a list of the archive files we expect to build from libraries
# Note that this path will typically contain a double-slash at the WORK_DIR boundary; this must be
# preserved as it is used below to get the absolute path for the library.mk file correct.
#
LIBRARY_LIBS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)library.a)
# rules to build module objects
.PHONY: $(LIBRARY_LIBS)
$(LIBRARY_LIBS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_LIBS): mkfile = $(patsubst %library.a,%library.mk,$(relpath))
$(LIBRARY_LIBS): workdir = $(@D)
$(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
-C $(workdir) \
LIBRARY_WORK_DIR=$(workdir) \
LIBRARY_LIB=$@ \
LIBRARY_MK=$(mkfile) \
LIBRARY_NAME=$(lastword $(subst /, ,$(workdir))) \
library
# make a list of phony clean targets for modules
LIBRARY_CLEANS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)/clean)
# rules to clean modules
.PHONY: $(LIBRARY_CLEANS)
$(LIBRARY_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_CLEANS): mkfile = $(patsubst %clean,%library.mk,$(relpath))
$(LIBRARY_CLEANS):
@$(ECHO) %% cleaning using $(mkfile)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
LIBRARY_WORK_DIR=$(dir $@) \
LIBRARY_MK=$(mkfile) \
clean
################################################################################
# NuttX libraries and paths
################################################################################
@ -266,14 +317,18 @@ endif
#
# Add dependencies on anything in the ROMFS root
ROMFS_DEPS += $(wildcard \
(ROMFS_ROOT)/* \
(ROMFS_ROOT)/*/* \
(ROMFS_ROOT)/*/*/* \
(ROMFS_ROOT)/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/*/*)
ROMFS_IMG = $(WORK_DIR)romfs.img
ROMFS_FILES += $(wildcard \
$(ROMFS_ROOT)/* \
$(ROMFS_ROOT)/*/* \
$(ROMFS_ROOT)/*/*/* \
$(ROMFS_ROOT)/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/*/*)
ifeq ($(ROMFS_FILES),)
$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
endif
ROMFS_DEPS += $(ROMFS_FILES)
ROMFS_IMG = romfs.img
ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)
@ -413,8 +468,8 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN)
$(PRODUCT_BIN): $(PRODUCT_ELF)
$(call SYM_TO_BIN,$<,$@)
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS))
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS))
#
# Utility rules

169
makefiles/library.mk Normal file
View File

@ -0,0 +1,169 @@
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#
# Framework makefile for PX4 libraries
#
# This makefile is invoked by firmware.mk to build each of the linraries
# that will subsequently be linked into the firmware image.
#
# Applications are built as standard ar archives. Unlike modules,
# all public symbols in library objects are visible across the entire
# firmware stack.
#
# In general, modules should be preferred to libraries when possible.
# Libraries may also be pre-built.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the library's output directory.
#
#
# Variables that can be set by the library's library.mk:
#
#
# SRCS (optional)
#
# Lists the .c, cpp and .S files that should be compiled/assembled to
# produce the library.
#
# PREBUILT_LIB (optional)
#
# Names the prebuilt library in the source directory that should be
# linked into the firmware.
#
# INCLUDE_DIRS (optional, must be appended, ignored if SRCS not set)
#
# The list of directories searched for include files. If non-standard
# includes (e.g. those from another module) are required, paths to search
# can be added here.
#
#
#
# Variables visible to the library's library.mk:
#
# CONFIG
# BOARD
# LIBRARY_WORK_DIR
# LIBRARY_LIB
# LIBRARY_MK
# Anything set in setup.mk, board_$(BOARD).mk and the toolchain file.
# Anything exported from config_$(CONFIG).mk
#
################################################################################
# No user-serviceable parts below.
################################################################################
ifeq ($(LIBRARY_MK),)
$(error No library makefile specified)
endif
$(info %% LIBRARY_MK = $(LIBRARY_MK))
#
# Get the board/toolchain config
#
include $(PX4_MK_DIR)/board_$(BOARD).mk
#
# Get the library's config
#
include $(LIBRARY_MK)
LIBRARY_SRC := $(dir $(LIBRARY_MK))
$(info % LIBRARY_NAME = $(LIBRARY_NAME))
$(info % LIBRARY_SRC = $(LIBRARY_SRC))
$(info % LIBRARY_LIB = $(LIBRARY_LIB))
$(info % LIBRARY_WORK_DIR = $(LIBRARY_WORK_DIR))
#
# Things that, if they change, might affect everything
#
GLOBAL_DEPS += $(MAKEFILE_LIST)
################################################################################
# Build rules
################################################################################
#
# What we're going to build
#
library: $(LIBRARY_LIB)
ifneq ($(PREBUILT_LIB),)
VPATH = $(LIBRARY_SRC)
$(LIBRARY_LIB): $(PREBUILT_LIB) $(GLOBAL_DEPS)
@$(ECHO) "PREBUILT: $(PREBUILT_LIB)"
$(Q) $(COPY) $< $@
else
##
## Object files we will generate from sources
##
OBJS = $(addsuffix .o,$(SRCS))
#
# SRCS -> OBJS rules
#
$(OBJS): $(GLOBAL_DEPS)
vpath %.c $(LIBRARY_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
vpath %.cpp $(LIBRARY_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
vpath %.S $(LIBRARY_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#
# Built product rules
#
$(LIBRARY_LIB): $(OBJS) $(GLOBAL_DEPS)
$(call ARCHIVE,$@,$(OBJS))
endif
#
# Utility rules
#
clean:
$(Q) $(REMOVE) $(LIBRARY_LIB) $(OBJS)

39
makefiles/module.mk
View File

@ -35,10 +35,14 @@
# This makefile is invoked by firmware.mk to build each of the modules
# that will subsequently be linked into the firmware image.
#
# Applications are built as prelinked objects with a limited set of exported
# Modules are built as prelinked objects with a limited set of exported
# symbols, as the global namespace is shared between all modules. Normally an
# module will just export one or more <command>_main functions.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the module's output directory.
#
#
# Variables that can be set by the module's module.mk:
@ -179,26 +183,10 @@ CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
#
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
#
# Locate sources (allows relative source paths in module.mk)
#
define SRC_SEARCH
$(abspath $(firstword $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
endef
ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
ifneq ($(MISSING_SRCS),)
$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
endif
ifeq ($(ABS_SRCS),)
$(error $(MODULE_MK): nothing to compile in SRCS)
endif
#
# Object files we will generate from sources
#
OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
##
## Object files we will generate from sources
##
OBJS = $(addsuffix .o,$(SRCS))
#
# SRCS -> OBJS rules
@ -206,13 +194,16 @@ OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
$(OBJS): $(GLOBAL_DEPS)
$(filter %.c.o,$(OBJS)): $(MODULE_WORK_DIR)%.c.o: %.c $(GLOBAL_DEPS)
vpath %.c $(MODULE_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
$(filter %.cpp.o,$(OBJS)): $(MODULE_WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS)
vpath %.cpp $(MODULE_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
$(filter %.S.o,$(OBJS)): $(MODULE_WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
vpath %.S $(MODULE_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#

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

@ -144,6 +144,7 @@ CFLAGS = $(ARCHCFLAGS) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
$(EXTRADEFINES) \
$(EXTRACFLAGS) \
-fno-common \
$(addprefix -I,$(INCLUDE_DIRS))
@ -156,18 +157,22 @@ CXXFLAGS = $(ARCHCXXFLAGS) \
$(ARCHXXINCLUDES) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
$(EXTRADEFINES) \
-DCONFIG_WCHAR_BUILTIN \
$(EXTRADEFINES) \
$(EXTRACXXFLAGS) \
$(addprefix -I,$(INCLUDE_DIRS))
# Flags we pass to the assembler
#
AFLAGS = $(CFLAGS) -D__ASSEMBLY__
AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
$(EXTRADEFINES) \
$(EXTRAAFLAGS)
# Flags we pass to the linker
#
LDFLAGS += --warn-common \
--gc-sections \
$(EXTRALDFLAGS) \
$(addprefix -T,$(LDSCRIPT)) \
$(addprefix -L,$(LIB_DIRS))
@ -249,6 +254,20 @@ endef
# - relink the object and insert the binary file
# - edit symbol names to suit
#
# NOTE: exercise caution using this with absolute pathnames; it looks
# like the MinGW tools insert an extra _ in the binary symbol name; e.g.
# the path:
#
# /d/px4/firmware/Build/px4fmu_default.build/romfs.img
#
# is assigned symbols like:
#
# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
# when we would expect
#
# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
define BIN_SYM_PREFIX
_binary_$(subst /,_,$(subst .,_,$1))
endef
@ -262,4 +281,5 @@ define BIN_TO_OBJ
--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
$(Q) $(REMOVE) $2.c $2.c.o
endef

3
makefiles/upload.mk
View File

@ -30,9 +30,6 @@ upload-serial-px4fmu: $(BUNDLE) $(UPLOADER)
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
#
# JTAG firmware uploading with OpenOCD
#

28
nuttx/.gitignore vendored Normal file
View File

@ -0,0 +1,28 @@
*.a
.config
.config-e
.configX-e
.depend
.version
arch/arm/include/board
arch/arm/include/chip
arch/arm/src/board
arch/arm/src/chip
include/apps
include/arch
include/math.h
include/nuttx/config.h
include/nuttx/version.h
Make.defs
Make.dep
mkdeps
nuttx
nuttx-export.zip
nuttx.bin
nuttx.hex
setenv.sh
System.map
tools/mkconfig
tools/mkconfig.exe
tools/mkversion
tools/mkversion.exe

1
nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
View File

@ -1512,7 +1512,6 @@ static inline void stm32_ep0out_receive(FAR struct stm32_ep_s *privep, int bcnt)
DEBUGASSERT(privep && privep->ep.priv);
priv = (FAR struct stm32_usbdev_s *)privep->ep.priv;
DEBUGASSERT(priv->ep0state == EP0STATE_SETUP_OUT);
ullvdbg("EP0: bcnt=%d\n", bcnt);
usbtrace(TRACE_READ(EP0), bcnt);

4
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -647,10 +647,14 @@ CONFIG_DISABLE_POLL=n
# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
# 5.1234567
# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu)
# CONFIG_LIBC_STRERR - allow printing of error text
# CONFIG_LIBC_STRERR_SHORT - allow printing of short error text
#
CONFIG_NOPRINTF_FIELDWIDTH=n
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_HAVE_LONG_LONG=y
CONFIG_LIBC_STRERROR=n
CONFIG_LIBC_STRERROR_SHORT=n
#
# Allow for architecture optimized implementations

30
nuttx/libc/stdio/lib_libdtoa.c
View File

@ -43,6 +43,7 @@
/****************************************************************************
* Included Files
****************************************************************************/
#include <math.h>
/****************************************************************************
* Pre-processor Definitions
@ -104,6 +105,13 @@ static void zeroes(FAR struct lib_outstream_s *obj, int nzeroes)
* Private Functions
****************************************************************************/
static void lib_dtoa_string(FAR struct lib_outstream_s *obj, const char *str)
{
while (*str) {
obj->put(obj, *str++);
}
}
/****************************************************************************
* Name: lib_dtoa
*
@ -137,6 +145,20 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
int nchars; /* Number of characters to print */
int dsgn; /* Unused sign indicator */
int i;
bool done_decimal_point = false;
/* special handling for NaN and Infinity */
if (isnan(value)) {
lib_dtoa_string(obj, "NaN");
return;
}
if (isinf(value)) {
if (value < 0.0d) {
obj->put(obj, '-');
}
lib_dtoa_string(obj, "Infinity");
return;
}
/* Non-zero... positive or negative */
@ -178,6 +200,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
if (prec > 0 || IS_ALTFORM(flags))
{
obj->put(obj, '.');
done_decimal_point = true;
/* Always print at least one digit to the right of the decimal point. */
@ -203,6 +226,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
done_decimal_point = true;
/* Print any leading zeros to the right of the decimal point */
@ -249,6 +273,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
done_decimal_point = true;
/* Always print at least one digit to the right of the decimal
* point.
@ -285,8 +310,9 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
}
/* Finally, print any trailing zeroes */
zeroes(obj, prec);
if (done_decimal_point) {
zeroes(obj, prec);
}
/* Is this memory supposed to be freed or not? */

7
nuttx/libc/stdio/lib_libvsprintf.c
View File

@ -1215,7 +1215,7 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
fmt = FMT_RJUST;
width = 0;
#ifdef CONFIG_LIBC_FLOATINGPOINT
trunc = 0;
trunc = 6;
#endif
#endif
@ -1245,6 +1245,11 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
{
#ifndef CONFIG_NOPRINTF_FIELDWIDTH
fmt = FMT_RJUST0;
#ifdef CONFIG_LIBC_FLOATINGPOINT
if (IS_HASDOT(flags)) {
trunc = 0;
}
#endif
#endif
}
#if 0

8
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);

3
src/drivers/boards/px4fmu/module.mk
View File

@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \
px4fmu_init.c \
px4fmu_pwm_servo.c \
px4fmu_spi.c \
px4fmu_usb.c
px4fmu_usb.c \
px4fmu_led.c

27
src/drivers/boards/px4fmu/px4fmu_init.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -91,6 +91,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void)
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
/* configure LEDs (empty call to NuttX' ledinit) */
up_ledinit();
}
@ -178,11 +191,11 @@ __EXPORT int nsh_archinitialize(void)
(hrt_callout)stm32_serial_dma_poll,
NULL);
// initial LED state
// drv_led_start();
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_on(LED_BLUE);
/* Configure SPI-based devices */

28
src/drivers/boards/px4fmu/px4fmu_led.c
View File

@ -39,19 +39,27 @@
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
#include "up_internal.h"
#include "stm32_internal.h"
#include "px4fmu_internal.h"
__EXPORT void up_ledinit()
#include <arch/board/board.h>
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
__EXPORT void led_init()
{
/* Configure LED1-2 GPIOs for output */
@ -59,7 +67,7 @@ __EXPORT void up_ledinit()
stm32_configgpio(GPIO_LED2);
}
__EXPORT void up_ledon(int led)
__EXPORT void led_on(int led)
{
if (led == 0)
{
@ -73,7 +81,7 @@ __EXPORT void up_ledon(int led)
}
}
__EXPORT void up_ledoff(int led)
__EXPORT void led_off(int led)
{
if (led == 0)
{

9
src/drivers/device/i2c.h
View File

@ -53,6 +53,15 @@ namespace device __EXPORT
class __EXPORT I2C : public CDev
{
public:
/**
* Get the address
*/
uint16_t get_address() {
return _address;
}
protected:
/**
* The number of times a read or write operation will be retried on

61
src/drivers/drv_airspeed.h Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Airspeed driver interface.
* @author Simon Wilks
*/
#ifndef _DRV_AIRSPEED_H
#define _DRV_AIRSPEED_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define AIRSPEED_DEVICE_PATH "/dev/airspeed"
/*
* ioctl() definitions
*
* Airspeed drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _AIRSPEEDIOCBASE (0x7700)
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
#endif /* _DRV_AIRSPEED_H */

6
src/drivers/drv_pwm_output.h
View File

@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

832
src/drivers/ets_airspeed/ets_airspeed.cpp Normal file
View File

@ -0,0 +1,832 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ets_airspeed.cpp
* @author Simon Wilks
*
* Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
/* Default I2C bus */
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
/* I2C bus address */
#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */
/* Register address */
#define READ_CMD 0x07 /* Read the data */
/**
* The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
*/
#define MIN_ACCURATE_DIFF_PRES_PA 12
/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class ETSAirspeed : public device::I2C
{
public:
ETSAirspeed(int bus, int address = I2C_ADDRESS);
virtual ~ETSAirspeed();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
work_s _work;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
differential_pressure_s *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _diff_pres_offset;
orb_advert_t _airspeed_pub;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
ETSAirspeed::ETSAirspeed(int bus, int address) :
I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0),
_airspeed_pub(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")),
_comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
ETSAirspeed::~ETSAirspeed()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
}
int
ETSAirspeed::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct differential_pressure_s[_num_reports];
for (unsigned i = 0; i < _num_reports; i++)
_reports[i].max_differential_pressure_pa = 0;
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the airspeed topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]);
if (_airspeed_pub < 0)
debug("failed to create airspeed sensor object. Did you start uOrb?");
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
ETSAirspeed::probe()
{
return measure();
}
int
ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct differential_pressure_s *buf = new struct differential_pressure_s[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
ETSAirspeed::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct differential_pressure_s);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
return -ENOSPC;
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
} while (0);
return ret;
}
int
ETSAirspeed::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd = READ_CMD;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
ETSAirspeed::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 2);
if (ret < 0) {
log("error reading from sensor: %d", ret);
return ret;
}
uint16_t diff_pres_pa = val[1] << 8 | val[0];
param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa -= _diff_pres_offset;
}
// XXX we may want to smooth out the readings to remove noise.
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].differential_pressure_pa = diff_pres_pa;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
_reports[_next_report].max_differential_pressure_pa = diff_pres_pa;
}
/* announce the airspeed if needed, just publish else */
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
ETSAirspeed::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_DIFFPRESSURE};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
ETSAirspeed::stop()
{
work_cancel(HPWORK, &_work);
}
void
ETSAirspeed::cycle_trampoline(void *arg)
{
ETSAirspeed *dev = (ETSAirspeed *)arg;
dev->cycle();
}
void
ETSAirspeed::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&ETSAirspeed::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure())
log("measure error");
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&ETSAirspeed::cycle_trampoline,
this,
USEC2TICK(CONVERSION_INTERVAL));
}
void
ETSAirspeed::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
}
/**
* Local functions in support of the shell command.
*/
namespace ets_airspeed
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
ETSAirspeed *g_dev;
void start(int i2c_bus);
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start(int i2c_bus)
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new ETSAirspeed(i2c_bus);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void
stop()
{
if (g_dev != nullptr)
{
delete g_dev;
g_dev = nullptr;
}
else
{
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct differential_pressure_s report;
ssize_t sz;
int ret;
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
errx(1, "failed to set 2Hz poll rate");
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
errx(1, "timed out waiting for sensor data");
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
static void
ets_airspeed_usage()
{
fprintf(stderr, "usage: ets_airspeed [options] command\n");
fprintf(stderr, "options:\n");
fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT);
fprintf(stderr, "command:\n");
fprintf(stderr, "\tstart|stop|reset|test|info\n");
}
int
ets_airspeed_main(int argc, char *argv[])
{
int i2c_bus = PX4_I2C_BUS_DEFAULT;
int i;
for (i = 1; i < argc; i++) {
if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
if (argc > i + 1) {
i2c_bus = atoi(argv[i + 1]);
}
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
ets_airspeed::start(i2c_bus);
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
ets_airspeed::stop();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
ets_airspeed::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
ets_airspeed::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
ets_airspeed::info();
ets_airspeed_usage();
exit(0);
}

30
src/modules/attitude_estimator_ekf/Makefile → src/drivers/ets_airspeed/module.mk Executable file → Normal file
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -31,27 +31,11 @@
#
############################################################################
APPNAME = attitude_estimator_ekf
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
#
# Makefile to build the Eagle Tree Airspeed V3 driver.
#
CXXSRCS = attitude_estimator_ekf_main.cpp
MODULE_COMMAND = ets_airspeed
MODULE_STACKSIZE = 1024
CSRCS = attitude_estimator_ekf_params.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
codegen/mrdivide.c \
codegen/rdivide.c \
codegen/attitudeKalmanfilter_initialize.c \
codegen/attitudeKalmanfilter_terminate.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk
SRCS = ets_airspeed.cpp

11
src/drivers/gps/gps.cpp
View File

@ -285,6 +285,10 @@ GPS::task_main()
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
@ -301,6 +305,8 @@ GPS::task_main()
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_Helper->store_update_rates();
_Helper->reset_update_rates();
}
if (!_healthy) {
@ -372,7 +378,10 @@ GPS::print_info()
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
}
usleep(100000);

34
src/drivers/gps/gps_helper.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@ -36,9 +36,39 @@
#include <termios.h>
#include <errno.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "gps_helper.h"
/* @file gps_helper.cpp */
/**
* @file gps_helper.cpp
*/
float
GPS_Helper::get_position_update_rate()
{
return _rate_lat_lon;
}
float
GPS_Helper::get_velocity_update_rate()
{
return _rate_vel;
}
float
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
_rate_count_lat_lon = 0;
_interval_rate_start = hrt_absolute_time();
}
float
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
}
int
GPS_Helper::set_baudrate(const int &fd, unsigned baud)

23
src/drivers/gps/gps_helper.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@ -33,7 +33,9 @@
*
****************************************************************************/
/* @file gps_helper.h */
/**
* @file gps_helper.h
*/
#ifndef GPS_HELPER_H
#define GPS_HELPER_H
@ -44,9 +46,22 @@
class GPS_Helper
{
public:
virtual int configure(unsigned &baud) = 0;
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
float reset_update_rates();
float store_update_rates();
protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
float _rate_lat_lon;
float _rate_vel;
uint64_t _interval_rate_start;
};
#endif /* GPS_HELPER_H */

4
src/drivers/gps/mtk.cpp
View File

@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time
_rate_count_vel++;
_rate_count_lat_lon++;
return;
}

6
src/drivers/gps/mtk.h
View File

@ -87,14 +87,14 @@ class MTK : public GPS_Helper
public:
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
/**
* Handle the package once it has arrived

164
src/drivers/gps/ubx.cpp
View File

@ -60,7 +60,8 @@
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_waiting_for_ack(false)
_waiting_for_ack(false),
_disable_cmd_counter(0)
{
decode_init();
}
@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.clsID = UBX_CLASS_CFG;
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
/* For satelites info 1Hz is enough */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
/* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
// 0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
_waiting_for_ack = false;
return 0;
@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
return -1;
}
int
UBX::wait_for_ack(unsigned timeout)
{
_waiting_for_ack = true;
int ret = receive(timeout);
_waiting_for_ack = false;
return ret;
}
int
UBX::receive(unsigned timeout)
{
@ -498,6 +475,8 @@ UBX::handle_message()
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_rate_count_lat_lon++;
/* Add timestamp to finish the report */
_gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */
@ -653,6 +632,8 @@ UBX::handle_message()
_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
}
break;
@ -693,6 +674,12 @@ UBX::handle_message()
default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
if (_disable_cmd_counter++ == 0) {
// Don't attempt for every message to disable, some might not be disabled */
warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
return ret;
ret = -1;
break;
}
@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
message[length-1] = ck_b;
}
void
UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
{
for (unsigned i = 0; i < length; i++) {
ck_a = ck_a + message[i];
ck_b = ck_b + ck_a;
}
}
void
UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
struct ubx_cfg_msg_rate msg;
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
warnx("ubx: config write fail");
}
void
UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
{
struct ubx_header header;
uint8_t ck_a=0, ck_b=0;
header.sync1 = UBX_SYNC1;
header.sync2 = UBX_SYNC2;
header.msg_class = msg_class;
header.msg_id = msg_id;
header.length = size;
add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
// Configure receive check
_clsID_needed = msg_class;
_msgID_needed = msg_id;
write(_fd, (const char *)&header, sizeof(header));
write(_fd, (const char *)msg, size);
write(_fd, (const char *)&ck_a, 1);
write(_fd, (const char *)&ck_b, 1);
}

78
src/drivers/gps/ubx.h
View File

@ -65,26 +65,27 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
#define UBX_CFG_RATE_LENGTH 6
#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
@ -92,6 +93,14 @@
/** the structures of the binary packets */
#pragma pack(push, 1)
struct ubx_header {
uint8_t sync1;
uint8_t sync2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */
@ -274,11 +283,17 @@ typedef struct {
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t rate[6];
uint8_t rate;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t;
struct ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
// END the structures of the binary packets
// ************
@ -341,55 +356,64 @@ class UBX : public GPS_Helper
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b);
int parse_char(uint8_t b);
/**
* Handle the package once it has arrived
*/
int handle_message(void);
int handle_message(void);
/**
* Reset the parse state machine for a fresh start
*/
void decode_init(void);
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
void add_byte_to_checksum(uint8_t);
/**
* Add the two checksum bytes to an outgoing message
*/
void add_checksum_to_message(uint8_t* message, const unsigned length);
void add_checksum_to_message(uint8_t* message, const unsigned length);
/**
* Helper to send a config packet
*/
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
int _fd;
void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
ubx_message_class_t _message_class;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
ubx_message_class_t _message_class;
ubx_message_id_t _message_id;
unsigned _payload_size;
unsigned _payload_size;
uint8_t _disable_cmd_counter;
};
#endif /* UBX_H_ */

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

@ -1225,19 +1225,25 @@ start()
/* create the driver, attempt expansion bus first */
g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
if (g_dev != nullptr && OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (g_dev == nullptr)
if (g_dev == nullptr) {
g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
if (g_dev != nullptr && OK != g_dev->init()) {
goto fail;
}
}
#endif
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);

12
src/drivers/hott_telemetry/messages.c
View File

@ -42,9 +42,11 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
static int airspeed_sub = -1;
static int battery_sub = -1;
static int sensor_sub = -1;
@ -52,6 +54,7 @@ void messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void build_eam_response(uint8_t *buffer, int *size)
@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);

21
src/drivers/led/led.cpp
View File

@ -41,12 +41,17 @@
#include <drivers/device/device.h>
#include <drivers/drv_led.h>
/* Ideally we'd be able to get these from up_internal.h */
//#include <up_internal.h>
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void up_ledinit();
extern void up_ledon(int led);
extern void up_ledoff(int led);
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
class LED : device::CDev
@ -74,7 +79,7 @@ int
LED::init()
{
CDev::init();
up_ledinit();
led_init();
return 0;
}
@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case LED_ON:
up_ledon(arg);
led_on(arg);
break;
case LED_OFF:
up_ledoff(arg);
led_off(arg);
break;
default:

553
src/drivers/md25/md25.cpp Normal file
View File

@ -0,0 +1,553 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file md25.cpp
*
* Driver for MD25 I2C Motor Driver
*
* references:
* http://www.robot-electronics.co.uk/htm/md25tech.htm
* http://www.robot-electronics.co.uk/files/rpi_md25.c
*
*/
#include "md25.hpp"
#include <poll.h>
#include <stdio.h>
#include <systemlib/err.h>
#include <arch/board/board.h>
// registers
enum {
// RW: read/write
// R: read
REG_SPEED1_RW = 0,
REG_SPEED2_RW,
REG_ENC1A_R,
REG_ENC1B_R,
REG_ENC1C_R,
REG_ENC1D_R,
REG_ENC2A_R,
REG_ENC2B_R,
REG_ENC2C_R,
REG_ENC2D_R,
REG_BATTERY_VOLTS_R,
REG_MOTOR1_CURRENT_R,
REG_MOTOR2_CURRENT_R,
REG_SW_VERSION_R,
REG_ACCEL_RATE_RW,
REG_MODE_RW,
REG_COMMAND_RW,
};
MD25::MD25(const char *deviceName, int bus,
uint16_t address, uint32_t speed) :
I2C("MD25", deviceName, bus, address, speed),
_controlPoll(),
_actuators(NULL, ORB_ID(actuator_controls_0), 20),
_version(0),
_motor1Speed(0),
_motor2Speed(0),
_revolutions1(0),
_revolutions2(0),
_batteryVoltage(0),
_motor1Current(0),
_motor2Current(0),
_motorAccel(0),
_mode(MODE_UNSIGNED_SPEED),
_command(CMD_RESET_ENCODERS)
{
// setup control polling
_controlPoll.fd = _actuators.getHandle();
_controlPoll.events = POLLIN;
// if initialization fails raise an error, unless
// probing
int ret = I2C::init();
if (ret != OK) {
warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address);
}
// setup default settings, reset encoders
setMotor1Speed(0);
setMotor2Speed(0);
resetEncoders();
_setMode(MD25::MODE_UNSIGNED_SPEED);
setSpeedRegulation(true);
setTimeout(true);
}
MD25::~MD25()
{
}
int MD25::readData()
{
uint8_t sendBuf[1];
sendBuf[0] = REG_SPEED1_RW;
uint8_t recvBuf[17];
int ret = transfer(sendBuf, sizeof(sendBuf),
recvBuf, sizeof(recvBuf));
if (ret == OK) {
_version = recvBuf[REG_SW_VERSION_R];
_motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]);
_motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]);
_revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) +
(recvBuf[REG_ENC1B_R] << 16) +
(recvBuf[REG_ENC1C_R] << 8) +
recvBuf[REG_ENC1D_R]) / 360.0;
_revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) +
(recvBuf[REG_ENC2B_R] << 16) +
(recvBuf[REG_ENC2C_R] << 8) +
recvBuf[REG_ENC2D_R]) / 360.0;
_batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0;
_motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0;
_motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0;
_motorAccel = recvBuf[REG_ACCEL_RATE_RW];
_mode = e_mode(recvBuf[REG_MODE_RW]);
_command = e_cmd(recvBuf[REG_COMMAND_RW]);
}
return ret;
}
void MD25::status(char *string, size_t n)
{
snprintf(string, n,
"version:\t%10d\n" \
"motor 1 speed:\t%10.2f\n" \
"motor 2 speed:\t%10.2f\n" \
"revolutions 1:\t%10.2f\n" \
"revolutions 2:\t%10.2f\n" \
"battery volts :\t%10.2f\n" \
"motor 1 current :\t%10.2f\n" \
"motor 2 current :\t%10.2f\n" \
"motor accel :\t%10d\n" \
"mode :\t%10d\n" \
"command :\t%10d\n",
getVersion(),
double(getMotor1Speed()),
double(getMotor2Speed()),
double(getRevolutions1()),
double(getRevolutions2()),
double(getBatteryVolts()),
double(getMotor1Current()),
double(getMotor2Current()),
getMotorAccel(),
getMode(),
getCommand());
}
uint8_t MD25::getVersion()
{
return _version;
}
float MD25::getMotor1Speed()
{
return _motor1Speed;
}
float MD25::getMotor2Speed()
{
return _motor2Speed;
}
float MD25::getRevolutions1()
{
return _revolutions1;
}
float MD25::getRevolutions2()
{
return _revolutions2;
}
float MD25::getBatteryVolts()
{
return _batteryVoltage;
}
float MD25::getMotor1Current()
{
return _motor1Current;
}
float MD25::getMotor2Current()
{
return _motor2Current;
}
uint8_t MD25::getMotorAccel()
{
return _motorAccel;
}
MD25::e_mode MD25::getMode()
{
return _mode;
}
MD25::e_cmd MD25::getCommand()
{
return _command;
}
int MD25::resetEncoders()
{
return _writeUint8(REG_COMMAND_RW,
CMD_RESET_ENCODERS);
}
int MD25::_setMode(e_mode mode)
{
return _writeUint8(REG_MODE_RW,
mode);
}
int MD25::setSpeedRegulation(bool enable)
{
if (enable) {
return _writeUint8(REG_COMMAND_RW,
CMD_ENABLE_SPEED_REGULATION);
} else {
return _writeUint8(REG_COMMAND_RW,
CMD_DISABLE_SPEED_REGULATION);
}
}
int MD25::setTimeout(bool enable)
{
if (enable) {
return _writeUint8(REG_COMMAND_RW,
CMD_ENABLE_TIMEOUT);
} else {
return _writeUint8(REG_COMMAND_RW,
CMD_DISABLE_TIMEOUT);
}
}
int MD25::setDeviceAddress(uint8_t address)
{
uint8_t sendBuf[1];
sendBuf[0] = CMD_CHANGE_I2C_SEQ_0;
int ret = OK;
ret = transfer(sendBuf, sizeof(sendBuf),
nullptr, 0);
if (ret != OK) {
warnc(ret, "MD25::setDeviceAddress");
return ret;
}
usleep(5000);
sendBuf[0] = CMD_CHANGE_I2C_SEQ_1;
ret = transfer(sendBuf, sizeof(sendBuf),
nullptr, 0);
if (ret != OK) {
warnc(ret, "MD25::setDeviceAddress");
return ret;
}
usleep(5000);
sendBuf[0] = CMD_CHANGE_I2C_SEQ_2;
ret = transfer(sendBuf, sizeof(sendBuf),
nullptr, 0);
if (ret != OK) {
warnc(ret, "MD25::setDeviceAddress");
return ret;
}
return OK;
}
int MD25::setMotor1Speed(float value)
{
return _writeUint8(REG_SPEED1_RW,
_normToUint8(value));
}
int MD25::setMotor2Speed(float value)
{
return _writeUint8(REG_SPEED2_RW,
_normToUint8(value));
}
void MD25::update()
{
// wait for an actuator publication,
// check for exit condition every second
// note "::poll" is required to distinguish global
// poll from member function for driver
if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
// if new data, send to motors
if (_actuators.updated()) {
_actuators.update();
setMotor1Speed(_actuators.control[CH_SPEED_LEFT]);
setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]);
}
}
int MD25::probe()
{
uint8_t goodAddress = 0;
bool found = false;
int ret = OK;
// try initial address first, if good, then done
if (readData() == OK) return ret;
// try all other addresses
uint8_t testAddress = 0;
//printf("searching for MD25 address\n");
while (true) {
set_address(testAddress);
ret = readData();
if (ret == OK && !found) {
//printf("device found at address: 0x%X\n", testAddress);
if (!found) {
found = true;
goodAddress = testAddress;
}
}
if (testAddress > 254) {
break;
}
testAddress++;
}
if (found) {
set_address(goodAddress);
return OK;
} else {
set_address(0);
return ret;
}
}
int MD25::search()
{
uint8_t goodAddress = 0;
bool found = false;
int ret = OK;
// try all other addresses
uint8_t testAddress = 0;
//printf("searching for MD25 address\n");
while (true) {
set_address(testAddress);
ret = readData();
if (ret == OK && !found) {
printf("device found at address: 0x%X\n", testAddress);
if (!found) {
found = true;
goodAddress = testAddress;
}
}
if (testAddress > 254) {
break;
}
testAddress++;
}
if (found) {
set_address(goodAddress);
return OK;
} else {
set_address(0);
return ret;
}
}
int MD25::_writeUint8(uint8_t reg, uint8_t value)
{
uint8_t sendBuf[2];
sendBuf[0] = reg;
sendBuf[1] = value;
return transfer(sendBuf, sizeof(sendBuf),
nullptr, 0);
}
int MD25::_writeInt8(uint8_t reg, int8_t value)
{
uint8_t sendBuf[2];
sendBuf[0] = reg;
sendBuf[1] = value;
return transfer(sendBuf, sizeof(sendBuf),
nullptr, 0);
}
float MD25::_uint8ToNorm(uint8_t value)
{
// TODO, should go from 0 to 255
// possibly should handle this differently
return (value - 128) / 127.0;
}
uint8_t MD25::_normToUint8(float value)
{
if (value > 1) value = 1;
if (value < -1) value = -1;
// TODO, should go from 0 to 255
// possibly should handle this differently
return 127 * value + 128;
}
int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
{
printf("md25 test: starting\n");
// setup
MD25 md25("/dev/md25", bus, address);
// print status
char buf[200];
md25.status(buf, sizeof(buf));
printf("%s\n", buf);
// setup for test
md25.setSpeedRegulation(true);
md25.setTimeout(true);
float dt = 0.1;
float speed = 0.2;
float t = 0;
// motor 1 test
printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n");
t = 0;
while (true) {
t += dt;
md25.setMotor1Speed(speed);
md25.readData();
usleep(1000000 * dt);
if (md25.getRevolutions1() > 1) {
printf("finished 1 revolution fwd\n");
break;
}
if (t > 2.0f) break;
}
md25.setMotor1Speed(0);
printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
md25.resetEncoders();
t = 0;
while (true) {
t += dt;
md25.setMotor1Speed(-speed);
md25.readData();
usleep(1000000 * dt);
if (md25.getRevolutions1() < -1) {
printf("finished 1 revolution rev\n");
break;
}
if (t > 2.0f) break;
}
md25.setMotor1Speed(0);
printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1()));
md25.resetEncoders();
// motor 2 test
printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n");
t = 0;
while (true) {
t += dt;
md25.setMotor2Speed(speed);
md25.readData();
usleep(1000000 * dt);
if (md25.getRevolutions2() > 1) {
printf("finished 1 revolution fwd\n");
break;
}
if (t > 2.0f) break;
}
md25.setMotor2Speed(0);
printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
md25.resetEncoders();
t = 0;
while (true) {
t += dt;
md25.setMotor2Speed(-speed);
md25.readData();
usleep(1000000 * dt);
if (md25.getRevolutions2() < -1) {
printf("finished 1 revolution rev\n");
break;
}
if (t > 2.0f) break;
}
md25.setMotor2Speed(0);
printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2()));
md25.resetEncoders();
printf("Test complete\n");
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78

293
src/drivers/md25/md25.hpp Normal file
View File

@ -0,0 +1,293 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file md25.cpp
*
* Driver for MD25 I2C Motor Driver
*
* references:
* http://www.robot-electronics.co.uk/htm/md25tech.htm
* http://www.robot-electronics.co.uk/files/rpi_md25.c
*
*/
#pragma once
#include <poll.h>
#include <stdio.h>
#include <controllib/block/UOrbSubscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <drivers/device/i2c.h>
/**
* This is a driver for the MD25 motor controller utilizing the I2C interface.
*/
class MD25 : public device::I2C
{
public:
/**
* modes
*
* NOTE: this driver assumes we are always
* in mode 0!
*
* seprate speed mode:
* motor speed1 = speed1
* motor speed2 = speed2
* turn speed mode:
* motor speed1 = speed1 + speed2
* motor speed2 = speed2 - speed2
* unsigned:
* full rev (0), stop(128), full fwd (255)
* signed:
* full rev (-127), stop(0), full fwd (128)
*
* modes numbers:
* 0 : unsigned separate (default)
* 1 : signed separate
* 2 : unsigned turn
* 3 : signed turn
*/
enum e_mode {
MODE_UNSIGNED_SPEED = 0,
MODE_SIGNED_SPEED,
MODE_UNSIGNED_SPEED_TURN,
MODE_SIGNED_SPEED_TURN,
};
/** commands */
enum e_cmd {
CMD_RESET_ENCODERS = 32,
CMD_DISABLE_SPEED_REGULATION = 48,
CMD_ENABLE_SPEED_REGULATION = 49,
CMD_DISABLE_TIMEOUT = 50,
CMD_ENABLE_TIMEOUT = 51,
CMD_CHANGE_I2C_SEQ_0 = 160,
CMD_CHANGE_I2C_SEQ_1 = 170,
CMD_CHANGE_I2C_SEQ_2 = 165,
};
/** control channels */
enum e_channels {
CH_SPEED_LEFT = 0,
CH_SPEED_RIGHT
};
/**
* constructor
* @param deviceName the name of the device e.g. "/dev/md25"
* @param bus the I2C bus
* @param address the adddress on the I2C bus
* @param speed the speed of the I2C communication
*/
MD25(const char *deviceName,
int bus,
uint16_t address,
uint32_t speed = 100000);
/**
* deconstructor
*/
virtual ~MD25();
/**
* @return software version
*/
uint8_t getVersion();
/**
* @return speed of motor, normalized (-1, 1)
*/
float getMotor1Speed();
/**
* @return speed of motor 2, normalized (-1, 1)
*/
float getMotor2Speed();
/**
* @return number of rotations since reset
*/
float getRevolutions1();
/**
* @return number of rotations since reset
*/
float getRevolutions2();
/**
* @return battery voltage, volts
*/
float getBatteryVolts();
/**
* @return motor 1 current, amps
*/
float getMotor1Current();
/**
* @return motor 2 current, amps
*/
float getMotor2Current();
/**
* @return the motor acceleration
* controls motor speed change (1-10)
* accel rate | time for full fwd. to full rev.
* 1 | 6.375 s
* 2 | 1.6 s
* 3 | 0.675 s
* 5(default) | 1.275 s
* 10 | 0.65 s
*/
uint8_t getMotorAccel();
/**
* @return motor output mode
* */
e_mode getMode();
/**
* @return current command register value
*/
e_cmd getCommand();
/**
* resets the encoders
* @return non-zero -> error
* */
int resetEncoders();
/**
* enable/disable speed regulation
* @return non-zero -> error
*/
int setSpeedRegulation(bool enable);
/**
* set the timeout for the motors
* enable/disable timeout (motor stop)
* after 2 sec of no i2c messages
* @return non-zero -> error
*/
int setTimeout(bool enable);
/**
* sets the device address
* can only be done with one MD25
* on the bus
* @return non-zero -> error
*/
int setDeviceAddress(uint8_t address);
/**
* set motor 1 speed
* @param normSpeed normalize speed between -1 and 1
* @return non-zero -> error
*/
int setMotor1Speed(float normSpeed);
/**
* set motor 2 speed
* @param normSpeed normalize speed between -1 and 1
* @return non-zero -> error
*/
int setMotor2Speed(float normSpeed);
/**
* main update loop that updates MD25 motor
* speeds based on actuator publication
*/
void update();
/**
* probe for device
*/
virtual int probe();
/**
* search for device
*/
int search();
/**
* read data from i2c
*/
int readData();
/**
* print status
*/
void status(char *string, size_t n);
private:
/** poll structure for control packets */
struct pollfd _controlPoll;
/** actuator controls subscription */
control::UOrbSubscription<actuator_controls_s> _actuators;
// local copy of data from i2c device
uint8_t _version;
float _motor1Speed;
float _motor2Speed;
float _revolutions1;
float _revolutions2;
float _batteryVoltage;
float _motor1Current;
float _motor2Current;
uint8_t _motorAccel;
e_mode _mode;
e_cmd _command;
// private methods
int _writeUint8(uint8_t reg, uint8_t value);
int _writeInt8(uint8_t reg, int8_t value);
float _uint8ToNorm(uint8_t value);
uint8_t _normToUint8(float value);
/**
* set motor control mode,
* this driver assumed we are always in mode 0
* so we don't let the user change the mode
* @return non-zero -> error
*/
int _setMode(e_mode);
};
// unit testing
int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78

264
src/drivers/md25/md25_main.cpp Normal file
View File

@ -0,0 +1,264 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @ file md25.cpp
*
* Driver for MD25 I2C Motor Driver
*
* references:
* http://www.robot-electronics.co.uk/htm/md25tech.htm
* http://www.robot-electronics.co.uk/files/rpi_md25.c
*
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <arch/board/board.h>
#include "md25.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int md25_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int md25_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int md25_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("md25 already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("md25",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
md25_thread_main,
(const char **)argv);
exit(0);
}
if (!strcmp(argv[1], "test")) {
if (argc < 4) {
printf("usage: md25 test bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
md25Test(deviceName, bus, address);
exit(0);
}
if (!strcmp(argv[1], "probe")) {
if (argc < 4) {
printf("usage: md25 probe bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
int ret = md25.probe();
if (ret == OK) {
printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address());
} else {
printf("MD25 not found on bus %d\n", bus);
}
exit(0);
}
if (!strcmp(argv[1], "search")) {
if (argc < 3) {
printf("usage: md25 search bus\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
md25.search();
exit(0);
}
if (!strcmp(argv[1], "change_address")) {
if (argc < 5) {
printf("usage: md25 change_address bus old_address new_address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t old_address = strtoul(argv[3], nullptr, 0);
uint8_t new_address = strtoul(argv[4], nullptr, 0);
MD25 md25(deviceName, bus, old_address);
int ret = md25.setDeviceAddress(new_address);
if (ret == OK) {
printf("MD25 new address set to 0x%X\n", new_address);
} else {
printf("MD25 failed to set address to 0x%X\n", new_address);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmd25 app is running\n");
} else {
printf("\tmd25 app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int md25_thread_main(int argc, char *argv[])
{
printf("[MD25] starting\n");
if (argc < 5) {
// extra md25 in arg list since this is a thread
printf("usage: md25 start bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[3], nullptr, 0);
uint8_t address = strtoul(argv[4], nullptr, 0);
// start
MD25 md25("/dev/md25", bus, address);
thread_running = true;
// loop
while (!thread_should_exit) {
md25.update();
}
// exit
printf("[MD25] exiting.\n");
thread_running = false;
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78

42
src/drivers/md25/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# MD25 I2C Based Motor Controller
# http://www.robot-electronics.co.uk/htm/md25tech.htm
#
MODULE_COMMAND = md25
SRCS = md25.cpp \
md25_main.cpp

1442
src/drivers/mkblctrl/mkblctrl.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

42
src/drivers/mkblctrl/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012,2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Interface driver for the Mikrokopter BLCtrl
#
MODULE_COMMAND = mkblctrl
SRCS = mkblctrl.cpp
INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common

11
src/drivers/px4fmu/fmu.cpp
View File

@ -574,7 +574,11 @@ PX4FMU::task_main()
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
up_pwm_servo_arm(aa.armed && !aa.lockdown);
bool set_armed = aa.armed && !aa.lockdown;
if (set_armed != _armed) {
_armed = set_armed;
up_pwm_servo_arm(set_armed);
}
}
#ifdef FMU_HAVE_PPM
@ -675,6 +679,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
up_pwm_servo_arm(false);
break;

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

@ -108,6 +108,14 @@ public:
*/
int set_update_rate(int rate);
/**
* Set the battery current scaling and bias
*
* @param amp_per_volt
* @param amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Print the current status of IO
*/
@ -151,6 +159,10 @@ private:
bool _primary_pwm_device; ///< true if we are the default PWM output
float _battery_amp_per_volt;
float _battery_amp_bias;
float _battery_mamphour_total;
uint64_t _battery_last_timestamp;
/**
* Trampoline to the worker task
@ -314,6 +326,10 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
_primary_pwm_device(false)
{
/* we need this potentially before it could be set in task_main */
@ -400,7 +416,7 @@ PX4IO::init()
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
@ -484,10 +500,9 @@ PX4IO::init()
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_ARM_OK |
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
/* publish RC config to IO */
ret = io_set_rc_config();
@ -686,16 +701,18 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_ARM_OK;
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
if (vstatus.flag_vector_flight_mode_ok) {
set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (vstatus.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
@ -884,11 +901,22 @@ PX4IO::io_get_status()
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
/* current scaling should be to cA in order to avoid limiting at 65A */
battery_status.current_a = regs[3] / 100.f;
/*
regs[3] contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/* this requires integration over time - not currently implemented */
battery_status.discharged_mah = -1.0f;
/*
integrate battery over time to get total mAh used
*/
if (_battery_last_timestamp != 0) {
_battery_mamphour_total += battery_status.current_a *
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
}
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
@ -1245,9 +1273,17 @@ PX4IO::print_status()
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
printf("vbatt %u ibatt %u\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
/* now clear alarms */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
printf("vbatt %u ibatt %u vbatt scale %u\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@ -1279,19 +1315,15 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
printf("controls");
for (unsigned i = 0; i < _max_controls; i++)
@ -1326,21 +1358,27 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_ARM:
/* set the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
break;
case PWM_SERVO_SET_ARM_OK:
/* set the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
break;
case PWM_SERVO_CLEAR_ARM_OK:
/* clear the 'OK to arm' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
break;
case PWM_SERVO_DISARM:
/* clear the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */
if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
} else {
ret = -EINVAL;
}
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE: {
@ -1525,6 +1563,12 @@ PX4IO::set_update_rate(int rate)
return 0;
}
void
PX4IO::set_battery_current_scaling(float amp_per_volt, float amp_bias)
{
_battery_amp_per_volt = amp_per_volt;
_battery_amp_bias = amp_bias;
}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
@ -1662,6 +1706,18 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "current")) {
if (g_dev != nullptr) {
if ((argc > 3)) {
g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3]));
} else {
errx(1, "missing argument (apm_per_volt, amp_offset)");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@ -1789,5 +1845,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
}

17
src/drivers/px4io/uploader.cpp
View File

@ -127,6 +127,8 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
close(_io_fd);
_io_fd = -1;
return -EIO;
}
@ -145,18 +147,25 @@ PX4IO_Uploader::upload(const char *filenames[])
if (filename == NULL) {
log("no firmware found");
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);
close(_io_fd);
_io_fd = -1;
return -errno;
}
fw_size = st.st_size;
if (_fw_fd == -1)
if (_fw_fd == -1) {
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
/* do the usual program thing - allow for failure */
for (unsigned retries = 0; retries < 1; retries++) {
@ -167,6 +176,8 @@ PX4IO_Uploader::upload(const char *filenames[])
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
close(_io_fd);
_io_fd = -1;
return -EIO;
}
}
@ -178,6 +189,8 @@ PX4IO_Uploader::upload(const char *filenames[])
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
close(_io_fd);
_io_fd = -1;
return OK;
}
}
@ -221,6 +234,8 @@ PX4IO_Uploader::upload(const char *filenames[])
}
close(_fw_fd);
close(_io_fd);
_io_fd = -1;
return ret;
}

474
src/examples/fixedwing_control/main.c Normal file
View File

@ -0,0 +1,474 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file main.c
* Implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller flying manual attitude control or auto waypoint control.
* There is no need to touch any other system components to extend / modify the
* complete control architecture.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
/* process-specific header files */
#include "params.h"
/* Prototypes */
/**
* Daemon management function.
*/
__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
*/
int fixedwing_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
/* Variables */
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
/*
* The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector
* is structured as:
*
* Control Group 0 (attitude):
*
* 0 - roll (-1..+1)
* 1 - pitch (-1..+1)
* 2 - yaw (-1..+1)
* 3 - thrust ( 0..+1)
* 4 - flaps (-1..+1)
* ...
*
* Control Group 1 (payloads / special):
*
* ...
*/
/*
* Calculate roll error and apply P gain
*/
float roll_err = att->roll - att_sp->roll_body;
actuators->control[0] = roll_err * p.roll_p;
/*
* Calculate pitch error and apply P gain
*/
float pitch_err = att->pitch - att_sp->pitch_body;
actuators->control[1] = pitch_err * p.pitch_p;
}
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
{
/*
* Calculate heading error of current position to desired position
*/
/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
/* apply control gain */
att_sp->roll_body = yaw_err * p.hdng_p;
}
/* Main Thread */
int fixedwing_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user (warnx prints a line, including an appended\n, with variable arguments */
warnx("[example fixedwing control] started");
/* initialize parameters, first the handles, then the values */
parameters_init(&ph);
parameters_update(&ph, &p);
/* declare and safely initialize all structs to zero */
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct vehicle_global_position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
while (!thread_should_exit) {
/*
* Wait for a sensor or param update, check for exit condition every 500 ms.
* This means that the execution will block here without consuming any resources,
* but will continue to execute the very moment a new attitude measurement or
* a param update is published. So no latency in contrast to the polling
* design pattern (do not confuse the poll() system call with polling).
*
* This design pattern makes the controller also agnostic of the attitude
* update speed - it runs as fast as the attitude updates with minimal latency.
*/
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, this will not really happen in practice */
warnx("poll error");
} else if (ret == 0) {
/* no return value = nothing changed for 500 ms, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag (uORB API requirement) */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* if a param update occured, re-read our parameters */
parameters_update(&ph, &p);
}
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
/* Check if there is a new position measurement or position setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (att.R_valid) {
speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
} else {
speed_body[0] = 0;
speed_body[1] = 0;
speed_body[2] = 0;
warnx("Did not get a valid R\n");
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* simple heading control */
control_heading(&global_pos, &global_sp, &att, &att_sp);
/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
actuators.control[1] = 0.0f;
actuators.control[2] = 0.0f;
/* simple attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* sanity check and publish actuator outputs */
if (isfinite(actuators.control[0]) &&
isfinite(actuators.control[1]) &&
isfinite(actuators.control[2]) &&
isfinite(actuators.control[3])) {
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
}
}
}
printf("[ex_fixedwing_control] exiting, stopping all motors.\n");
thread_running = false;
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
fflush(stdout);
return 0;
}
/* Startup Functions */
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
exit(1);
}
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int ex_fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ex_fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tex_fixedwing_control is running\n");
} else {
printf("\tex_fixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}

26
src/modules/mathlib/CMSIS/module.mk → src/examples/fixedwing_control/module.mk
View File

@ -32,28 +32,10 @@
############################################################################
#
# ARM CMSIS DSP library
# Fixedwing Attitude Control Demo / Example Application
#
#
# Find sources
#
DSPLIB_SRCDIR := $(PX4_MODULE_SRC)/modules/mathlib/CMSIS
ABS_SRCS := $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
MODULE_COMMAND = ex_fixedwing_control
INCLUDE_DIRS += $(DSPLIB_SRCDIR)/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM3/Include
# Suppress some warnings that ARM should fix, but haven't.
EXTRADEFINES += -Wno-unsuffixed-float-constants \
-Wno-sign-compare \
-Wno-shadow \
-Wno-float-equal \
-Wno-unused-variable
#
# Override the default visibility for symbols, since the CMSIS DSPLib doesn't
# have anything we can use to mark exported symbols.
#
DEFAULT_VISIBILITY = YES
SRCS = main.c \
params.c

77
src/examples/fixedwing_control/params.c Normal file
View File

@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file params.c
*
* Parameters for fixedwing demo
*/
#include "params.h"
/* controller parameters, use max. 15 characters for param name! */
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
int parameters_init(struct param_handles *h)
{
/* PID parameters */
h->hdng_p = param_find("EXFW_HDNG_P");
h->roll_p = param_find("EXFW_ROLL_P");
h->pitch_p = param_find("EXFW_PITCH_P");
return OK;
}
int parameters_update(const struct param_handles *h, struct params *p)
{
param_get(h->hdng_p, &(p->hdng_p));
param_get(h->roll_p, &(p->roll_p));
param_get(h->pitch_p, &(p->pitch_p));
return OK;
}

65
src/examples/fixedwing_control/params.h Normal file
View File

@ -0,0 +1,65 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file params.h
*
* Definition of parameters for fixedwing example
*/
#include <systemlib/param/param.h>
struct params {
float hdng_p;
float roll_p;
float pitch_p;
};
struct param_handles {
param_t hdng_p;
param_t roll_p;
param_t pitch_p;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct param_handles *h, struct params *p);

2
src/examples/px4_daemon_app/module.mk
View File

@ -37,4 +37,4 @@
MODULE_COMMAND = px4_daemon_app
SRCS = px4_daemon_app.c
SRCS = px4_daemon_app.c

2
src/examples/px4_mavlink_debug/module.mk
View File

@ -37,4 +37,4 @@
MODULE_COMMAND = px4_mavlink_debug
SRCS = px4_mavlink_debug.c
SRCS = px4_mavlink_debug.c

2
src/examples/px4_simple_app/module.mk
View File

@ -37,4 +37,4 @@
MODULE_COMMAND = px4_simple_app
SRCS = px4_simple_app.c
SRCS = px4_simple_app.c

45
src/include/mavlink/mavlink_log.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@ -47,27 +47,42 @@
*/
#include <sys/ioctl.h>
/*
/**
* The mavlink log device node; must be opened before messages
* can be logged.
*/
#define MAVLINK_LOG_DEVICE "/dev/mavlink"
/**
* The maximum string length supported.
*/
#define MAVLINK_LOG_MAXLEN 50
#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1)
#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2)
#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3)
#ifdef __cplusplus
extern "C" {
#endif
__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
#ifdef __cplusplus
}
#endif
/*
* The va_args implementation here is not beautiful, but obviously we run into the same issues
* the GCC devs saw, and are using their solution:
*
* http://gcc.gnu.org/onlinedocs/cpp/Variadic-Macros.html
*/
/**
* Send a mavlink emergency message.
*
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#else
#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#endif
#define mavlink_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
/**
* Send a mavlink critical message.
@ -75,11 +90,7 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#else
#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#endif
#define mavlink_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
/**
* Send a mavlink info message.
@ -87,14 +98,10 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#else
#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#endif
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
struct mavlink_logmessage {
char text[51];
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
};
@ -116,5 +123,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
#endif

414
src/modules/commander/accelerometer_calibration.c Normal file
View File

@ -0,0 +1,414 @@
/*
* accelerometer_calibration.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* Transform acceleration vector to true orientation and scale
*
* * * * Model * * *
* accel_corr = accel_T * (accel_raw - accel_offs)
*
* accel_corr[3] - fully corrected acceleration vector in body frame
* accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
* accel_raw[3] - raw acceleration vector
* accel_offs[3] - acceleration offset vector
*
* * * * Calibration * * *
*
* Reference vectors
* accel_corr_ref[6][3] = [ g 0 0 ] // nose up
* | -g 0 0 | // nose down
* | 0 g 0 | // left side down
* | 0 -g 0 | // right side down
* | 0 0 g | // on back
* [ 0 0 -g ] // level
* accel_raw_ref[6][3]
*
* accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
*
* 6 reference vectors * 3 axes = 18 equations
* 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
*
* Find accel_offs
*
* accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
*
*
* Find accel_T
*
* 9 unknown constants
* need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
*
* accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
*
* Solve separate system for each row of accel_T:
*
* accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
*
* A * x = b
*
* x = [ accel_T[0][i] ]
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
* a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
*
* Matrix A is common for all three systems:
* A = [ a[0][0] a[0][1] a[0][2] ]
* | a[2][0] a[2][1] a[2][2] |
* [ a[4][0] a[4][1] a[4][2] ]
*
* x = A^-1 * b
*
* accel_T = A^-1 * g
* g = 9.80665
*/
#include "accelerometer_calibration.h"
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
#include <systemlib/conversions.h>
#include <mavlink/mavlink_log.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_error();
sleep(2);
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
}
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
/* reset existing calibration */
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
close(fd);
if (OK != ioctl_res) {
warn("ERROR: failed to set scale / offsets for accel");
return ERROR;
}
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
while (true) {
bool done = true;
char str[80];
int str_ptr;
str_ptr = sprintf(str, "keep vehicle still:");
for (int i = 0; i < 6; i++) {
if (!data_collected[i]) {
str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
done = false;
}
}
if (done)
break;
mavlink_log_info(mavlink_fd, str);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0)
return ERROR;
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
mavlink_log_info(mavlink_fd, str);
data_collected[orient] = true;
tune_confirm();
}
close(sensor_combined_sub);
/* calculate offsets and rotation+scale matrix */
float accel_T[3][3];
int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (res != 0) {
mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
return ERROR;
}
/* convert accel transform matrix to scales,
* rotation part of transform matrix is not used by now
*/
for (int i = 0; i < 3; i++) {
accel_scale[i] = accel_T[i][i];
}
return OK;
}
/*
* Wait for vehicle become still and detect it's orientation.
*
* @return 0..5 according to orientation when vehicle is still and ready for measurements,
* ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
*/
int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
struct sensor_combined_s sensor;
/* exponential moving average of accel */
float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
/* max-hold dispersion of accel */
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/
float ema_len = 0.2f;
/* set "still" threshold to 0.1 m/s^2 */
float still_thr2 = pow(0.1f, 2);
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
int64_t still_time = 2000000;
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
hrt_abstime timeout = 30000000;
hrt_abstime t_timeout = t_start + timeout;
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
float w = dt / ema_len;
for (int i = 0; i < 3; i++) {
accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > accel_disp[i])
accel_disp[i] = d;
}
/* still detector with hysteresis */
if ( accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
accel_disp[2] < still_thr2 ) {
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if ((int64_t) t - (int64_t) t_still > still_time) {
/* vehicle is still, exit from the loop to detection of its orientation */
break;
}
}
} else if ( accel_disp[0] > still_thr2 * 2.0f ||
accel_disp[1] > still_thr2 * 2.0f ||
accel_disp[2] > still_thr2 * 2.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "moving...");
t_still = 0;
}
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "ERROR: poll failure");
return -3;
}
if (t > t_timeout) {
mavlink_log_info(mavlink_fd, "ERROR: timeout");
return -1;
}
}
if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
return 0; // [ g, 0, 0 ]
if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
return 1; // [ -g, 0, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
return 2; // [ 0, g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
return 3; // [ 0, -g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
return 4; // [ 0, 0, g ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
return -2; // Can't detect orientation
}
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
int count = 0;
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
while (count < samples_num) {
int poll_ret = poll(fds, 1, 1000);
if (poll_ret == 1) {
struct sensor_combined_s sensor;
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
for (int i = 0; i < 3; i++)
accel_sum[i] += sensor.accelerometer_m_s2[i];
count++;
} else {
return ERROR;
}
}
for (int i = 0; i < 3; i++) {
accel_avg[i] = accel_sum[i] / count;
}
return OK;
}
int mat_invert3(float src[3][3], float dst[3][3]) {
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0)
return ERROR; // Singular matrix
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
return OK;
}
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
/* calculate offsets */
for (int i = 0; i < 3; i++) {
accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
}
/* fill matrix A for linear equations system*/
float mat_A[3][3];
memset(mat_A, 0, sizeof(mat_A));
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
float a = accel_ref[i * 2][j] - accel_offs[j];
mat_A[i][j] = a;
}
}
/* calculate inverse matrix for A */
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK)
return ERROR;
/* copy results to accel_T */
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
/* simplify matrices mult because b has only one non-zero element == g at index i */
accel_T[j][i] = mat_A_inv[j][i] * g;
}
}
return OK;
}

16
src/modules/commander/accelerometer_calibration.h Normal file
View File

@ -0,0 +1,16 @@
/*
* accelerometer_calibration.h
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef ACCELEROMETER_CALIBRATION_H_
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/topics/vehicle_status.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */

191
src/modules/commander/commander.c
View File

@ -94,7 +94,7 @@
#include <drivers/drv_baro.h>
#include "calibration_routines.h"
#include "accelerometer_calibration.h"
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
@ -158,7 +158,6 @@ static int led_off(int led);
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
@ -666,126 +665,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
mavlink_log_info(mavlink_fd, "keep it level and still");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
const int calibration_count = 2500;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
int calibration_counter = 0;
float accel_offset[3] = {0.0f, 0.0f, 0.0f};
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
accel_offset[0] += raw.accelerometer_m_s2[0];
accel_offset[1] += raw.accelerometer_m_s2[1];
accel_offset[2] += raw.accelerometer_m_s2[2];
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
return;
}
}
accel_offset[0] = accel_offset[0] / calibration_count;
accel_offset[1] = accel_offset[1] / calibration_count;
accel_offset[2] = accel_offset[2] / calibration_count;
if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
/* add the removed length from x / y to z, since we induce a scaling issue else */
float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]);
/* if length is correct, zero results here */
accel_offset[2] = accel_offset[2] + total_len;
float scale = 9.80665f / total_len;
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(scale))
|| param_set(param_find("SENS_ACC_YSCALE"), &(scale))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!");
}
fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offset[0],
scale,
accel_offset[1],
scale,
accel_offset[2],
scale,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)");
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_sensor_combined);
}
void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
@ -797,22 +676,22 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
const int calibration_count = 2500;
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float airspeed_offset = 0.0f;
float diff_pres_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
airspeed_offset += differential_pressure.voltage;
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
} else if (poll_ret == 0) {
@ -822,11 +701,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
}
}
airspeed_offset = airspeed_offset / calibration_count;
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(airspeed_offset)) {
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
@ -856,7 +735,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
status->flag_preflight_airspeed_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_differential_pressure);
close(diff_pres_sub);
}
@ -1040,7 +919,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "CMD starting accel cal");
tune_confirm();
do_accel_calibration(status_pub, &current_status);
do_accel_calibration(status_pub, &current_status, mavlink_fd);
tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished accel cal");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
@ -1477,10 +1356,10 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
memset(&differential_pressure, 0, sizeof(differential_pressure));
uint64_t last_differential_pressure_time = 0;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
memset(&diff_pres, 0, sizeof(diff_pres));
uint64_t last_diff_pres_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@ -1535,11 +1414,11 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
orb_check(differential_pressure_sub, &new_data);
orb_check(diff_pres_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
last_differential_pressure_time = differential_pressure.timestamp;
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
last_diff_pres_time = diff_pres.timestamp;
}
orb_check(cmd_sub, &new_data);
@ -1624,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
current_status.state_machine == SYSTEM_STATE_AUTO ||
current_status.state_machine == SYSTEM_STATE_MANUAL)) {
/* armed */
led_toggle(LED_BLUE);
/* armed, solid */
led_on(LED_AMBER);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */
led_toggle(LED_BLUE);
led_toggle(LED_AMBER);
}
/* toggle error led at 5 Hz in HIL mode */
if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
/* GPS lock */
led_on(LED_BLUE);
} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* no GPS lock, but GPS module is aquiring lock */
led_toggle(LED_BLUE);
}
} else {
/* no GPS info, don't light the blue led */
led_off(LED_BLUE);
}
/* toggle GPS led at 5 Hz in HIL mode */
if (current_status.flag_hil_enabled) {
/* hil enabled */
led_toggle(LED_AMBER);
led_toggle(LED_BLUE);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
/* toggle error (red) at 5 Hz on low battery or error */
/* toggle arming (red) at 5 Hz on low battery or error */
led_toggle(LED_AMBER);
} else {
@ -1754,7 +1651,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
current_status.flag_airspeed_valid = true;
} else {

10
src/modules/commander/module.mk
View File

@ -35,7 +35,9 @@
# Main system state machine
#
MODULE_COMMAND = commander
SRCS = commander.c \
state_machine_helper.c \
calibration_routines.c
MODULE_COMMAND = commander
SRCS = commander.c \
state_machine_helper.c \
calibration_routines.c \
accelerometer_calibration.c

5
src/modules/commander/state_machine_helper.c
View File

@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
{
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* XXX allow arming by external components on multicopters only if not yet armed by RC */
/* XXX allow arming only if core sensors are ok */
armed.ready_to_arm = true;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);

64
src/modules/controllib/fixedwing.cpp
View File

@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r)
void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
_rudder = _r2Rdr.update(rCmd -
_rudder = outputScale*_r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r)
float p, float q, float r, float outputScale)
{
_aileron = _p2Ail.update(
_aileron = outputScale*_p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = _q2Elv.update(
_elevator = outputScale*_q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r);
_yawDamper.update(rCmd, r, outputScale);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_theLimit(this, "THE"),
_vLimit(this, "V"),
// altitude/roc hold
// altitude/climb rate hold
_h2Thr(this, "H2THR"),
_roc2Thr(this, "ROC2THR"),
_cr2Thr(this, "CR2THR"),
// guidance block
_guide(this, ""),
// block params
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
_trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
_trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
_vCmd(this, "V_CMD"),
_rocMax(this, "ROC_MAX"),
_crMax(this, "CR_MAX"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update()
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update()
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(
_vLimit.update(_vCmd.get()) - v));
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
float velocityRatio = _trimV.get()/v;
float outputScale = velocityRatio*velocityRatio;
// this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
outputScale);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@ -280,13 +291,18 @@ void BlockMultiModeBacksideAutopilot::update()
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _roc2Thr.update(
//_rocMax.get()*_manual.pitch - _pos.vz);
//float dThrottle = _cr2Thr.update(
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update()
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
float vCmd = _vLimit.update(_manual.throttle *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd

12
src/modules/controllib/fixedwing.hpp
View File

@ -193,7 +193,7 @@ public:
* good idea to declare a member to store the temporary
* variable.
*/
void update(float rCmd, float r);
void update(float rCmd, float r, float outputScale = 1.0);
/**
* Rudder output value accessor
@ -226,7 +226,8 @@ public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
float p, float q, float r);
float p, float q, float r,
float outputScale = 1.0);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
@ -310,9 +311,9 @@ private:
BlockLimit _theLimit;
BlockLimit _vLimit;
// altitude/ roc hold
// altitude/ climb rate hold
BlockPID _h2Thr;
BlockPID _roc2Thr;
BlockPID _cr2Thr;
// guidance
BlockWaypointGuidance _guide;
@ -322,8 +323,9 @@ private:
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _rocMax;
BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;

3
src/modules/fixedwing_backside/module.mk
View File

@ -37,4 +37,5 @@
MODULE_COMMAND = fixedwing_backside
SRCS = fixedwing_backside_main.cpp
SRCS = fixedwing_backside_main.cpp \
params.c

17
src/modules/fixedwing_backside/params.c
View File

@ -59,13 +59,14 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
// rate of climb
// this is what rate of climb is commanded (in m/s)
// when the pitch stick is fully defelcted in simple mode
PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f);
// rate of climb -> thr
PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
// climb rate -> thr
PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID
PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s

159
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c
View File

@ -1,159 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_f32.c
*
* Description: Vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
#include <math.h>
/**
* @ingroup groupMath
*/
/**
* @defgroup BasicAbs Vector Absolute Value
*
* Computes the absolute value of a vector on an element-by-element basis.
*
* <pre>
* pDst[n] = abs(pSrcA[n]), 0 <= n < blockSize.
* </pre>
*
* The operation can be done in-place by setting the input and output pointers to the same buffer.
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Floating-point vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*/
void arm_abs_f32(
float32_t * pSrc,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t in1, in2, in3, in4; /* temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute and then store the results in the destination buffer. */
/* read sample from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
/* find absolute value */
in1 = fabsf(in1);
/* read sample from source */
in4 = *(pSrc + 3);
/* find absolute value */
in2 = fabsf(in2);
/* read sample from source */
*pDst = in1;
/* find absolute value */
in3 = fabsf(in3);
/* find absolute value */
in4 = fabsf(in4);
/* store result to destination */
*(pDst + 1) = in2;
/* store result to destination */
*(pDst + 2) = in3;
/* store result to destination */
*(pDst + 3) = in4;
/* Update source pointer to process next sampels */
pSrc += 4u;
/* Update destination pointer to process next sampels */
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute and then store the results in the destination buffer. */
*pDst++ = fabsf(*pSrc++);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/

173
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c
View File

@ -1,173 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q15.c
*
* Description: Q15 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q15 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.
*/
void arm_abs_q15(
q15_t * pSrc,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q15_t in1; /* Input value1 */
q15_t in2; /* Input value2 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Read two inputs */
in1 = *pSrc++;
in2 = *pSrc++;
/* Store the Absolute result in the destination buffer by packing the two values, in a single cycle */
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ =
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
#else
*__SIMD32(pDst)++ =
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
in1 = *pSrc++;
in2 = *pSrc++;
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ =
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
#else
*__SIMD32(pDst)++ =
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in1 = *pSrc++;
/* Calculate absolute value of input and then store the result in the destination buffer. */
*pDst++ = (in1 > 0) ? in1 : __QSUB16(0, in1);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
q15_t in; /* Temporary input variable */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in = *pSrc++;
/* Calculate absolute value of input and then store the result in the destination buffer. */
*pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAbs group
*/

125
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c
View File

@ -1,125 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q31.c
*
* Description: Q31 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q31 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.
*/
void arm_abs_q31(
q31_t * pSrc,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t in; /* Input value */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
*pDst++ = (in1 > 0) ? in1 : __QSUB(0, in1);
*pDst++ = (in2 > 0) ? in2 : __QSUB(0, in2);
*pDst++ = (in3 > 0) ? in3 : __QSUB(0, in3);
*pDst++ = (in4 > 0) ? in4 : __QSUB(0, in4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute value of the input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
in = *pSrc++;
*pDst++ = (in > 0) ? in : ((in == 0x80000000) ? 0x7fffffff : -in);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/

152
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c
View File

@ -1,152 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q7.c
*
* Description: Q7 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q7 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* \par Conditions for optimum performance
* Input and output buffers should be aligned by 32-bit
*
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.
*/
void arm_abs_q7(
q7_t * pSrc,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q7_t in; /* Input value1 */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4; /* temporary input variables */
q31_t out1, out2, out3, out4; /* temporary output variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Read inputs */
in1 = (q31_t) * pSrc;
in2 = (q31_t) * (pSrc + 1);
in3 = (q31_t) * (pSrc + 2);
/* find absolute value */
out1 = (in1 > 0) ? in1 : __QSUB8(0, in1);
/* read input */
in4 = (q31_t) * (pSrc + 3);
/* find absolute value */
out2 = (in2 > 0) ? in2 : __QSUB8(0, in2);
/* store result to destination */
*pDst = (q7_t) out1;
/* find absolute value */
out3 = (in3 > 0) ? in3 : __QSUB8(0, in3);
/* find absolute value */
out4 = (in4 > 0) ? in4 : __QSUB8(0, in4);
/* store result to destination */
*(pDst + 1) = (q7_t) out2;
/* store result to destination */
*(pDst + 2) = (q7_t) out3;
/* store result to destination */
*(pDst + 3) = (q7_t) out4;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
blkCnt = blockSize;
#endif // #define ARM_MATH_CM0
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in = *pSrc++;
/* Store the Absolute result in the destination buffer */
*pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? 0x7f : -in);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/

145
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c
View File

@ -1,145 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_f32.c
*
* Description: Floating-point vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup BasicAdd Vector Addition
*
* Element-by-element addition of two vectors.
*
* <pre>
* pDst[n] = pSrcA[n] + pSrcB[n], 0 <= n < blockSize.
* </pre>
*
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Floating-point vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*/
void arm_add_f32(
float32_t * pSrcA,
float32_t * pSrcB,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t inA1, inA2, inA3, inA4; /* temporary input variabels */
float32_t inB1, inB2, inB3, inB4; /* temporary input variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
/* read four inputs from sourceA and four inputs from sourceB */
inA1 = *pSrcA;
inB1 = *pSrcB;
inA2 = *(pSrcA + 1);
inB2 = *(pSrcB + 1);
inA3 = *(pSrcA + 2);
inB3 = *(pSrcB + 2);
inA4 = *(pSrcA + 3);
inB4 = *(pSrcB + 3);
/* C = A + B */
/* add and store result to destination */
*pDst = inA1 + inB1;
*(pDst + 1) = inA2 + inB2;
*(pDst + 2) = inA3 + inB3;
*(pDst + 3) = inA4 + inB4;
/* update pointers to process next samples */
pSrcA += 4u;
pSrcB += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (*pSrcA++) + (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAdd group
*/

135
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q15.c
*
* Description: Q15 vector addition
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q15 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
*/
void arm_add_q15(
q15_t * pSrcA,
q15_t * pSrcB,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inB1, inB2;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
inA1 = *__SIMD32(pSrcA)++;
inA2 = *__SIMD32(pSrcA)++;
inB1 = *__SIMD32(pSrcB)++;
inB2 = *__SIMD32(pSrcB)++;
*__SIMD32(pDst)++ = __QADD16(inA1, inB1);
*__SIMD32(pDst)++ = __QADD16(inA2, inB2);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q15_t) __QADD16(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ + *pSrcB++), 16);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/

143
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c
View File

@ -1,143 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q31.c
*
* Description: Q31 vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q31 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.
*/
void arm_add_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inA3, inA4;
q31_t inB1, inB2, inB3, inB4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
inA1 = *pSrcA++;
inA2 = *pSrcA++;
inB1 = *pSrcB++;
inB2 = *pSrcB++;
inA3 = *pSrcA++;
inA4 = *pSrcA++;
inB3 = *pSrcB++;
inB4 = *pSrcB++;
*pDst++ = __QADD(inA1, inB1);
*pDst++ = __QADD(inA2, inB2);
*pDst++ = __QADD(inA3, inB3);
*pDst++ = __QADD(inA4, inB4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ + *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/

129
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c
View File

@ -1,129 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q7.c
*
* Description: Q7 vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q7 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
*/
void arm_add_q7(
q7_t * pSrcA,
q7_t * pSrcB,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q7_t) __SSAT(*pSrcA++ + *pSrcB++, 8);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ + *pSrcB++, 8);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/

125
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c
View File

@ -1,125 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_dot_prod_f32.c
*
* Description: Floating-point dot product.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup dot_prod Vector Dot Product
*
* Computes the dot product of two vectors.
* The vectors are multiplied element-by-element and then summed.
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup dot_prod
* @{
*/
/**
* @brief Dot product of floating-point vectors.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[in] blockSize number of samples in each vector
* @param[out] *result output result returned here
* @return none.
*/
void arm_dot_prod_f32(
float32_t * pSrcA,
float32_t * pSrcB,
uint32_t blockSize,
float32_t * result)
{
float32_t sum = 0.0f; /* Temporary result storage */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer */
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
sum += (*pSrcA++) * (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
/* Store the result back in the destination buffer */
*result = sum;
}
/**
* @} end of dot_prod group
*/

135
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_dot_prod_q15.c
*
* Description: Q15 dot product.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup dot_prod
* @{
*/
/**
* @brief Dot product of Q15 vectors.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[in] blockSize number of samples in each vector
* @param[out] *result output result returned here
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The intermediate multiplications are in 1.15 x 1.15 = 2.30 format and these
* results are added to a 64-bit accumulator in 34.30 format.
* Nonsaturating additions are used and given that there are 33 guard bits in the accumulator
* there is no risk of overflow.
* The return result is in 34.30 format.
*/
void arm_dot_prod_q15(
q15_t * pSrcA,
q15_t * pSrcB,
uint32_t blockSize,
q63_t * result)
{
q63_t sum = 0; /* Temporary result storage */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the results in a temporary buffer. */
sum = __SMLALD(*pSrcA++, *pSrcB++, sum);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the results in a temporary buffer. */
sum += (q63_t) ((q31_t) * pSrcA++ * *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
/* Store the result in the destination buffer in 34.30 format */
*result = sum;
}
/**
* @} end of dot_prod group
*/

138
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c
View File

@ -1,138 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_dot_prod_q31.c
*
* Description: Q31 dot product.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup dot_prod
* @{
*/
/**
* @brief Dot product of Q31 vectors.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[in] blockSize number of samples in each vector
* @param[out] *result output result returned here
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The intermediate multiplications are in 1.31 x 1.31 = 2.62 format and these
* are truncated to 2.48 format by discarding the lower 14 bits.
* The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format.
* There are 15 guard bits in the accumulator and there is no risk of overflow as long as
* the length of the vectors is less than 2^16 elements.
* The return result is in 16.48 format.
*/
void arm_dot_prod_q31(
q31_t * pSrcA,
q31_t * pSrcB,
uint32_t blockSize,
q63_t * result)
{
q63_t sum = 0; /* Temporary result storage */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inA3, inA4;
q31_t inB1, inB2, inB3, inB4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
inA1 = *pSrcA++;
inA2 = *pSrcA++;
inA3 = *pSrcA++;
inA4 = *pSrcA++;
inB1 = *pSrcB++;
inB2 = *pSrcB++;
inB3 = *pSrcB++;
inB4 = *pSrcB++;
sum += ((q63_t) inA1 * inB1) >> 14u;
sum += ((q63_t) inA2 * inB2) >> 14u;
sum += ((q63_t) inA3 * inB3) >> 14u;
sum += ((q63_t) inA4 * inB4) >> 14u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
sum += ((q63_t) * pSrcA++ * *pSrcB++) >> 14u;
/* Decrement the loop counter */
blkCnt--;
}
/* Store the result in the destination buffer in 16.48 format */
*result = sum;
}
/**
* @} end of dot_prod group
*/

154
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c
View File

@ -1,154 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_dot_prod_q7.c
*
* Description: Q7 dot product.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup dot_prod
* @{
*/
/**
* @brief Dot product of Q7 vectors.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[in] blockSize number of samples in each vector
* @param[out] *result output result returned here
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The intermediate multiplications are in 1.7 x 1.7 = 2.14 format and these
* results are added to an accumulator in 18.14 format.
* Nonsaturating additions are used and there is no danger of wrap around as long as
* the vectors are less than 2^18 elements long.
* The return result is in 18.14 format.
*/
void arm_dot_prod_q7(
q7_t * pSrcA,
q7_t * pSrcB,
uint32_t blockSize,
q31_t * result)
{
uint32_t blkCnt; /* loop counter */
q31_t sum = 0; /* Temporary variables to store output */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t input1, input2; /* Temporary variables to store input */
q31_t inA1, inA2, inB1, inB2; /* Temporary variables to store input */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* read 4 samples at a time from sourceA */
input1 = *__SIMD32(pSrcA)++;
/* read 4 samples at a time from sourceB */
input2 = *__SIMD32(pSrcB)++;
/* extract two q7_t samples to q15_t samples */
inA1 = __SXTB16(__ROR(input1, 8));
/* extract reminaing two samples */
inA2 = __SXTB16(input1);
/* extract two q7_t samples to q15_t samples */
inB1 = __SXTB16(__ROR(input2, 8));
/* extract reminaing two samples */
inB2 = __SXTB16(input2);
/* multiply and accumulate two samples at a time */
sum = __SMLAD(inA1, inB1, sum);
sum = __SMLAD(inA2, inB2, sum);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Dot product and then store the results in a temporary buffer. */
sum = __SMLAD(*pSrcA++, *pSrcB++, sum);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Dot product and then store the results in a temporary buffer. */
sum += (q31_t) ((q15_t) * pSrcA++ * *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
/* Store the result in the destination buffer in 18.14 format */
*result = sum;
}
/**
* @} end of dot_prod group
*/

172
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c
View File

@ -1,172 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_mult_f32.c
*
* Description: Floating-point vector multiplication.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.5 2010/04/26
* incorporated review comments and updated with latest CMSIS layer
*
* Version 0.0.3 2010/03/10
* Initial version
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup BasicMult Vector Multiplication
*
* Element-by-element multiplication of two vectors.
*
* <pre>
* pDst[n] = pSrcA[n] * pSrcB[n], 0 <= n < blockSize.
* </pre>
*
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup BasicMult
* @{
*/
/**
* @brief Floating-point vector multiplication.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*/
void arm_mult_f32(
float32_t * pSrcA,
float32_t * pSrcB,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counters */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t inA1, inA2, inA3, inA4; /* temporary input variables */
float32_t inB1, inB2, inB3, inB4; /* temporary input variables */
float32_t out1, out2, out3, out4; /* temporary output variables */
/* loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and store the results in output buffer */
/* read sample from sourceA */
inA1 = *pSrcA;
/* read sample from sourceB */
inB1 = *pSrcB;
/* read sample from sourceA */
inA2 = *(pSrcA + 1);
/* read sample from sourceB */
inB2 = *(pSrcB + 1);
/* out = sourceA * sourceB */
out1 = inA1 * inB1;
/* read sample from sourceA */
inA3 = *(pSrcA + 2);
/* read sample from sourceB */
inB3 = *(pSrcB + 2);
/* out = sourceA * sourceB */
out2 = inA2 * inB2;
/* read sample from sourceA */
inA4 = *(pSrcA + 3);
/* store result to destination buffer */
*pDst = out1;
/* read sample from sourceB */
inB4 = *(pSrcB + 3);
/* out = sourceA * sourceB */
out3 = inA3 * inB3;
/* store result to destination buffer */
*(pDst + 1) = out2;
/* out = sourceA * sourceB */
out4 = inA4 * inB4;
/* store result to destination buffer */
*(pDst + 2) = out3;
/* store result to destination buffer */
*(pDst + 3) = out4;
/* update pointers to process next samples */
pSrcA += 4u;
pSrcB += 4u;
pDst += 4u;
/* Decrement the blockSize loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and store the results in output buffer */
*pDst++ = (*pSrcA++) * (*pSrcB++);
/* Decrement the blockSize loop counter */
blkCnt--;
}
}
/**
* @} end of BasicMult group
*/

152
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c
View File

@ -1,152 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_mult_q15.c
*
* Description: Q15 vector multiplication.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.5 2010/04/26
* incorporated review comments and updated with latest CMSIS layer
*
* Version 0.0.3 2010/03/10
* Initial version
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicMult
* @{
*/
/**
* @brief Q15 vector multiplication
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
*/
void arm_mult_q15(
q15_t * pSrcA,
q15_t * pSrcB,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counters */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inB1, inB2; /* temporary input variables */
q15_t out1, out2, out3, out4; /* temporary output variables */
q31_t mul1, mul2, mul3, mul4; /* temporary variables */
/* loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* read two samples at a time from sourceA */
inA1 = *__SIMD32(pSrcA)++;
/* read two samples at a time from sourceB */
inB1 = *__SIMD32(pSrcB)++;
/* read two samples at a time from sourceA */
inA2 = *__SIMD32(pSrcA)++;
/* read two samples at a time from sourceB */
inB2 = *__SIMD32(pSrcB)++;
/* multiply mul = sourceA * sourceB */
mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
mul2 = (q31_t) ((q15_t) inA1 * (q15_t) inB1);
mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16));
mul4 = (q31_t) ((q15_t) inA2 * (q15_t) inB2);
/* saturate result to 16 bit */
out1 = (q15_t) __SSAT(mul1 >> 15, 16);
out2 = (q15_t) __SSAT(mul2 >> 15, 16);
out3 = (q15_t) __SSAT(mul3 >> 15, 16);
out4 = (q15_t) __SSAT(mul4 >> 15, 16);
/* store the result */
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
*__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
#else
*__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
*__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
#endif // #ifndef ARM_MATH_BIG_ENDIAN
/* Decrement the blockSize loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and store the result in the destination buffer */
*pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
/* Decrement the blockSize loop counter */
blkCnt--;
}
}
/**
* @} end of BasicMult group
*/

143
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c
View File

@ -1,143 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_mult_q31.c
*
* Description: Q31 vector multiplication.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.5 2010/04/26
* incorporated review comments and updated with latest CMSIS layer
*
* Version 0.0.3 2010/03/10
* Initial version
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicMult
* @{
*/
/**
* @brief Q31 vector multiplication.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.
*/
void arm_mult_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counters */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inA3, inA4; /* temporary input variables */
q31_t inB1, inB2, inB3, inB4; /* temporary input variables */
q31_t out1, out2, out3, out4; /* temporary output variables */
/* loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and then store the results in the destination buffer. */
inA1 = *pSrcA++;
inA2 = *pSrcA++;
inA3 = *pSrcA++;
inA4 = *pSrcA++;
inB1 = *pSrcB++;
inB2 = *pSrcB++;
inB3 = *pSrcB++;
inB4 = *pSrcB++;
out1 = ((q63_t) inA1 * inB1) >> 32;
out2 = ((q63_t) inA2 * inB2) >> 32;
out3 = ((q63_t) inA3 * inB3) >> 32;
out4 = ((q63_t) inA4 * inB4) >> 32;
out1 = __SSAT(out1, 31);
out2 = __SSAT(out2, 31);
out3 = __SSAT(out3, 31);
out4 = __SSAT(out4, 31);
*pDst++ = out1 << 1u;
*pDst++ = out2 << 1u;
*pDst++ = out3 << 1u;
*pDst++ = out4 << 1u;
/* Decrement the blockSize loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and then store the results in the destination buffer. */
*pDst++ =
(q31_t) clip_q63_to_q31(((q63_t) (*pSrcA++) * (*pSrcB++)) >> 31);
/* Decrement the blockSize loop counter */
blkCnt--;
}
}
/**
* @} end of BasicMult group
*/

128
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c
View File

@ -1,128 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_mult_q7.c
*
* Description: Q7 vector multiplication.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
*
* Version 0.0.5 2010/04/26
* incorporated review comments and updated with latest CMSIS layer
*
* Version 0.0.3 2010/03/10 DP
* Initial version
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicMult
* @{
*/
/**
* @brief Q7 vector multiplication
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
*/
void arm_mult_q7(
q7_t * pSrcA,
q7_t * pSrcB,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counters */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q7_t out1, out2, out3, out4; /* Temporary variables to store the product */
/* loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and store the results in temporary variables */
out1 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
out2 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
out3 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
out4 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
/* Store the results of 4 inputs in the destination buffer in single cycle by packing */
*__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
/* Decrement the blockSize loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * B */
/* Multiply the inputs and store the result in the destination buffer */
*pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
/* Decrement the blockSize loop counter */
blkCnt--;
}
}
/**
* @} end of BasicMult group
*/

137
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c
View File

@ -1,137 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_negate_f32.c
*
* Description: Negates floating-point vectors.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup negate Vector Negate
*
* Negates the elements of a vector.
*
* <pre>
* pDst[n] = -pSrc[n], 0 <= n < blockSize.
* </pre>
*/
/**
* @addtogroup negate
* @{
*/
/**
* @brief Negates the elements of a floating-point vector.
* @param[in] *pSrc points to the input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*/
void arm_negate_f32(
float32_t * pSrc,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t in1, in2, in3, in4; /* temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* read inputs from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
in4 = *(pSrc + 3);
/* negate the input */
in1 = -in1;
in2 = -in2;
in3 = -in3;
in4 = -in4;
/* store the result to destination */
*pDst = in1;
*(pDst + 1) = in2;
*(pDst + 2) = in3;
*(pDst + 3) = in4;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the results in the destination buffer. */
*pDst++ = -*pSrc++;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of negate group
*/

137
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c
View File

@ -1,137 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_negate_q15.c
*
* Description: Negates Q15 vectors.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup negate
* @{
*/
/**
* @brief Negates the elements of a Q15 vector.
* @param[in] *pSrc points to the input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* \par Conditions for optimum performance
* Input and output buffers should be aligned by 32-bit
*
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.
*/
void arm_negate_q15(
q15_t * pSrc,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q15_t in;
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2; /* Temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = -A */
/* Read two inputs at a time */
in1 = _SIMD32_OFFSET(pSrc);
in2 = _SIMD32_OFFSET(pSrc + 2);
/* negate two samples at a time */
in1 = __QSUB16(0, in1);
/* negate two samples at a time */
in2 = __QSUB16(0, in2);
/* store the result to destination 2 samples at a time */
_SIMD32_OFFSET(pDst) = in1;
/* store the result to destination 2 samples at a time */
_SIMD32_OFFSET(pDst + 2) = in2;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the result in the destination buffer. */
in = *pSrc++;
*pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of negate group
*/

124
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c
View File

@ -1,124 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_negate_q31.c
*
* Description: Negates Q31 vectors.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup negate
* @{
*/
/**
* @brief Negates the elements of a Q31 vector.
* @param[in] *pSrc points to the input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.
*/
void arm_negate_q31(
q31_t * pSrc,
q31_t * pDst,
uint32_t blockSize)
{
q31_t in; /* Temporary variable */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the results in the destination buffer. */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
*pDst++ = __QSUB(0, in1);
*pDst++ = __QSUB(0, in2);
*pDst++ = __QSUB(0, in3);
*pDst++ = __QSUB(0, in4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the result in the destination buffer. */
in = *pSrc++;
*pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of negate group
*/

120
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c
View File

@ -1,120 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_negate_q7.c
*
* Description: Negates Q7 vectors.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup negate
* @{
*/
/**
* @brief Negates the elements of a Q7 vector.
* @param[in] *pSrc points to the input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.
*/
void arm_negate_q7(
q7_t * pSrc,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q7_t in;
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t input; /* Input values1-4 */
q31_t zero = 0x00000000;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = -A */
/* Read four inputs */
input = *__SIMD32(pSrc)++;
/* Store the Negated results in the destination buffer in a single cycle by packing the results */
*__SIMD32(pDst)++ = __QSUB8(zero, input);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = -A */
/* Negate and then store the results in the destination buffer. */ \
in = *pSrc++;
*pDst++ = (in == (q7_t) 0x80) ? 0x7f : -in;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of negate group
*/

158
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c
View File

@ -1,158 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_offset_f32.c
*
* Description: Floating-point vector offset.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup offset Vector Offset
*
* Adds a constant offset to each element of a vector.
*
* <pre>
* pDst[n] = pSrc[n] + offset, 0 <= n < blockSize.
* </pre>
*
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup offset
* @{
*/
/**
* @brief Adds a constant offset to a floating-point vector.
* @param[in] *pSrc points to the input vector
* @param[in] offset is the offset to be added
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*/
void arm_offset_f32(
float32_t * pSrc,
float32_t offset,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t in1, in2, in3, in4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination buffer. */
/* read samples from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
/* add offset to input */
in1 = in1 + offset;
/* read samples from source */
in3 = *(pSrc + 2);
/* add offset to input */
in2 = in2 + offset;
/* read samples from source */
in4 = *(pSrc + 3);
/* add offset to input */
in3 = in3 + offset;
/* store result to destination */
*pDst = in1;
/* add offset to input */
in4 = in4 + offset;
/* store result to destination */
*(pDst + 1) = in2;
/* store result to destination */
*(pDst + 2) = in3;
/* store result to destination */
*(pDst + 3) = in4;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = (*pSrc++) + offset;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of offset group
*/

131
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c
View File

@ -1,131 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_offset_q15.c
*
* Description: Q15 vector offset.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup offset
* @{
*/
/**
* @brief Adds a constant offset to a Q15 vector.
* @param[in] *pSrc points to the input vector
* @param[in] offset is the offset to be added
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated.
*/
void arm_offset_q15(
q15_t * pSrc,
q15_t offset,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t offset_packed; /* Offset packed to 32 bit */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* Offset is packed to 32 bit in order to use SIMD32 for addition */
offset_packed = __PKHBT(offset, offset, 16);
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination buffer, 2 samples at a time. */
*__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
*__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination buffer. */
*pDst++ = (q15_t) __QADD16(*pSrc++, offset);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination buffer. */
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrc++ + offset), 16);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of offset group
*/

135
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c
View File

@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_offset_q31.c
*
* Description: Q31 vector offset.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup offset
* @{
*/
/**
* @brief Adds a constant offset to a Q31 vector.
* @param[in] *pSrc points to the input vector
* @param[in] offset is the offset to be added
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated.
*/
void arm_offset_q31(
q31_t * pSrc,
q31_t offset,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination buffer. */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
*pDst++ = __QADD(in1, offset);
*pDst++ = __QADD(in2, offset);
*pDst++ = __QADD(in3, offset);
*pDst++ = __QADD(in4, offset);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = __QADD(*pSrc++, offset);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of offset group
*/

130
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c
View File

@ -1,130 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_offset_q7.c
*
* Description: Q7 vector offset.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup offset
* @{
*/
/**
* @brief Adds a constant offset to a Q7 vector.
* @param[in] *pSrc points to the input vector
* @param[in] offset is the offset to be added
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q7 range [0x80 0x7F] are saturated.
*/
void arm_offset_q7(
q7_t * pSrc,
q7_t offset,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t offset_packed; /* Offset packed to 32 bit */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* Offset is packed to 32 bit in order to use SIMD32 for addition */
offset_packed = __PACKq7(offset, offset, offset, offset);
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the results in the destination bufferfor 4 samples at a time. */
*__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrc)++, offset_packed);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = (q7_t) __SSAT(*pSrc++ + offset, 8);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + offset */
/* Add offset and then store the result in the destination buffer. */
*pDst++ = (q7_t) __SSAT((q15_t) * pSrc++ + offset, 8);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of offset group
*/

161
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c
View File

@ -1,161 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_scale_f32.c
*
* Description: Multiplies a floating-point vector by a scalar.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup scale Vector Scale
*
* Multiply a vector by a scalar value. For floating-point data, the algorithm used is:
*
* <pre>
* pDst[n] = pSrc[n] * scale, 0 <= n < blockSize.
* </pre>
*
* In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by
* a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>.
* The shift allows the gain of the scaling operation to exceed 1.0.
* The algorithm used with fixed-point data is:
*
* <pre>
* pDst[n] = (pSrc[n] * scaleFract) << shift, 0 <= n < blockSize.
* </pre>
*
* The overall scale factor applied to the fixed-point data is
* <pre>
* scale = scaleFract * 2^shift.
* </pre>
*/
/**
* @addtogroup scale
* @{
*/
/**
* @brief Multiplies a floating-point vector by a scalar.
* @param[in] *pSrc points to the input vector
* @param[in] scale scale factor to be applied
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*/
void arm_scale_f32(
float32_t * pSrc,
float32_t scale,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t in1, in2, in3, in4; /* temporary variabels */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the results in the destination buffer. */
/* read input samples from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
/* multiply with scaling factor */
in1 = in1 * scale;
/* read input sample from source */
in3 = *(pSrc + 2);
/* multiply with scaling factor */
in2 = in2 * scale;
/* read input sample from source */
in4 = *(pSrc + 3);
/* multiply with scaling factor */
in3 = in3 * scale;
in4 = in4 * scale;
/* store the result to destination */
*pDst = in1;
*(pDst + 1) = in2;
*(pDst + 2) = in3;
*(pDst + 3) = in4;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
*pDst++ = (*pSrc++) * scale;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of scale group
*/

157
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c
View File

@ -1,157 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_scale_q15.c
*
* Description: Multiplies a Q15 vector by a scalar.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup scale
* @{
*/
/**
* @brief Multiplies a Q15 vector by a scalar.
* @param[in] *pSrc points to the input vector
* @param[in] scaleFract fractional portion of the scale value
* @param[in] shift number of bits to shift the result by
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format.
* These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format.
*/
void arm_scale_q15(
q15_t * pSrc,
q15_t scaleFract,
int8_t shift,
q15_t * pDst,
uint32_t blockSize)
{
int8_t kShift = 15 - shift; /* shift to apply after scaling */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q15_t in1, in2, in3, in4;
q31_t inA1, inA2; /* Temporary variables */
q31_t out1, out2, out3, out4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* Reading 2 inputs from memory */
inA1 = *__SIMD32(pSrc)++;
inA2 = *__SIMD32(pSrc)++;
/* C = A * scale */
/* Scale the inputs and then store the 2 results in the destination buffer
* in single cycle by packing the outputs */
out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract);
out2 = (q31_t) ((q15_t) inA1 * scaleFract);
out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract);
out4 = (q31_t) ((q15_t) inA2 * scaleFract);
/* apply shifting */
out1 = out1 >> kShift;
out2 = out2 >> kShift;
out3 = out3 >> kShift;
out4 = out4 >> kShift;
/* saturate the output */
in1 = (q15_t) (__SSAT(out1, 16));
in2 = (q15_t) (__SSAT(out2, 16));
in3 = (q15_t) (__SSAT(out3, 16));
in4 = (q15_t) (__SSAT(out4, 16));
/* store the result to destination */
*__SIMD32(pDst)++ = __PKHBT(in2, in1, 16);
*__SIMD32(pDst)++ = __PKHBT(in4, in3, 16);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
*pDst++ = (q15_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 16));
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
*pDst++ = (q15_t) (__SSAT(((q31_t) * pSrc++ * scaleFract) >> kShift, 16));
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of scale group
*/

221
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c
View File

@ -1,221 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_scale_q31.c
*
* Description: Multiplies a Q31 vector by a scalar.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup scale
* @{
*/
/**
* @brief Multiplies a Q31 vector by a scalar.
* @param[in] *pSrc points to the input vector
* @param[in] scaleFract fractional portion of the scale value
* @param[in] shift number of bits to shift the result by
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format.
* These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format.
*/
void arm_scale_q31(
q31_t * pSrc,
q31_t scaleFract,
int8_t shift,
q31_t * pDst,
uint32_t blockSize)
{
int8_t kShift = shift + 1; /* Shift to apply after scaling */
int8_t sign = (kShift & 0x80);
uint32_t blkCnt; /* loop counter */
q31_t in, out;
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4; /* temporary input variables */
q31_t out1, out2, out3, out4; /* temporary output variabels */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
if(sign == 0u)
{
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* read four inputs from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
in4 = *(pSrc + 3);
/* multiply input with scaler value */
in1 = ((q63_t) in1 * scaleFract) >> 32;
in2 = ((q63_t) in2 * scaleFract) >> 32;
in3 = ((q63_t) in3 * scaleFract) >> 32;
in4 = ((q63_t) in4 * scaleFract) >> 32;
/* apply shifting */
out1 = in1 << kShift;
out2 = in2 << kShift;
/* saturate the results. */
if(in1 != (out1 >> kShift))
out1 = 0x7FFFFFFF ^ (in1 >> 31);
if(in2 != (out2 >> kShift))
out2 = 0x7FFFFFFF ^ (in2 >> 31);
out3 = in3 << kShift;
out4 = in4 << kShift;
*pDst = out1;
*(pDst + 1) = out2;
if(in3 != (out3 >> kShift))
out3 = 0x7FFFFFFF ^ (in3 >> 31);
if(in4 != (out4 >> kShift))
out4 = 0x7FFFFFFF ^ (in4 >> 31);
/* Store result destination */
*(pDst + 2) = out3;
*(pDst + 3) = out4;
/* Update pointers to process next sampels */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
}
else
{
kShift = -kShift;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* read four inputs from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
in4 = *(pSrc + 3);
/* multiply input with scaler value */
in1 = ((q63_t) in1 * scaleFract) >> 32;
in2 = ((q63_t) in2 * scaleFract) >> 32;
in3 = ((q63_t) in3 * scaleFract) >> 32;
in4 = ((q63_t) in4 * scaleFract) >> 32;
/* apply shifting */
out1 = in1 >> kShift;
out2 = in2 >> kShift;
out3 = in3 >> kShift;
out4 = in4 >> kShift;
/* Store result destination */
*pDst = out1;
*(pDst + 1) = out2;
*(pDst + 2) = out3;
*(pDst + 3) = out4;
/* Update pointers to process next sampels */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
in = *pSrc++;
in = ((q63_t) in * scaleFract) >> 32;
if(sign == 0)
{
out = in << kShift;
if(in != (out >> kShift))
out = 0x7FFFFFFF ^ (in >> 31);
}
else
{
out = in >> kShift;
}
*pDst++ = out;
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of scale group
*/

144
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c
View File

@ -1,144 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_scale_q7.c
*
* Description: Multiplies a Q7 vector by a scalar.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup scale
* @{
*/
/**
* @brief Multiplies a Q7 vector by a scalar.
* @param[in] *pSrc points to the input vector
* @param[in] scaleFract fractional portion of the scale value
* @param[in] shift number of bits to shift the result by
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.7 format.
* These are multiplied to yield a 2.14 intermediate result and this is shifted with saturation to 1.7 format.
*/
void arm_scale_q7(
q7_t * pSrc,
q7_t scaleFract,
int8_t shift,
q7_t * pDst,
uint32_t blockSize)
{
int8_t kShift = 7 - shift; /* shift to apply after scaling */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q7_t in1, in2, in3, in4, out1, out2, out3, out4; /* Temporary variables to store input & output */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* Reading 4 inputs from memory */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
/* C = A * scale */
/* Scale the inputs and then store the results in the temporary variables. */
out1 = (q7_t) (__SSAT(((in1) * scaleFract) >> kShift, 8));
out2 = (q7_t) (__SSAT(((in2) * scaleFract) >> kShift, 8));
out3 = (q7_t) (__SSAT(((in3) * scaleFract) >> kShift, 8));
out4 = (q7_t) (__SSAT(((in4) * scaleFract) >> kShift, 8));
/* Packing the individual outputs into 32bit and storing in
* destination buffer in single write */
*__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
*pDst++ = (q7_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 8));
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A * scale */
/* Scale the input and then store the result in the destination buffer. */
*pDst++ = (q7_t) (__SSAT((((q15_t) * pSrc++ * scaleFract) >> kShift), 8));
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of scale group
*/

243
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c
View File

@ -1,243 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_shift_q15.c
*
* Description: Shifts the elements of a Q15 vector by a specified number of bits.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup shift
* @{
*/
/**
* @brief Shifts the elements of a Q15 vector a specified number of bits.
* @param[in] *pSrc points to the input vector
* @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
*/
void arm_shift_q15(
q15_t * pSrc,
int8_t shiftBits,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
uint8_t sign; /* Sign of shiftBits */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q15_t in1, in2; /* Temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* Getting the sign of shiftBits */
sign = (shiftBits & 0x80);
/* If the shift value is positive then do right shift else left shift */
if(sign == 0u)
{
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* Read 2 inputs */
in1 = *pSrc++;
in2 = *pSrc++;
/* C = A << shiftBits */
/* Shift the inputs and then store the results in the destination buffer. */
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
__SSAT((in2 << shiftBits), 16), 16);
#else
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
__SSAT((in1 << shiftBits), 16), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
in1 = *pSrc++;
in2 = *pSrc++;
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
__SSAT((in2 << shiftBits), 16), 16);
#else
*__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
__SSAT((in1 << shiftBits), 16), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A << shiftBits */
/* Shift and then store the results in the destination buffer. */
*pDst++ = __SSAT((*pSrc++ << shiftBits), 16);
/* Decrement the loop counter */
blkCnt--;
}
}
else
{
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* Read 2 inputs */
in1 = *pSrc++;
in2 = *pSrc++;
/* C = A >> shiftBits */
/* Shift the inputs and then store the results in the destination buffer. */
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
(in2 >> -shiftBits), 16);
#else
*__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
(in1 >> -shiftBits), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
in1 = *pSrc++;
in2 = *pSrc++;
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
(in2 >> -shiftBits), 16);
#else
*__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
(in1 >> -shiftBits), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A >> shiftBits */
/* Shift the inputs and then store the results in the destination buffer. */
*pDst++ = (*pSrc++ >> -shiftBits);
/* Decrement the loop counter */
blkCnt--;
}
}
#else
/* Run the below code for Cortex-M0 */
/* Getting the sign of shiftBits */
sign = (shiftBits & 0x80);
/* If the shift value is positive then do right shift else left shift */
if(sign == 0u)
{
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A << shiftBits */
/* Shift and then store the results in the destination buffer. */
*pDst++ = __SSAT(((q31_t) * pSrc++ << shiftBits), 16);
/* Decrement the loop counter */
blkCnt--;
}
}
else
{
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A >> shiftBits */
/* Shift the inputs and then store the results in the destination buffer. */
*pDst++ = (*pSrc++ >> -shiftBits);
/* Decrement the loop counter */
blkCnt--;
}
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of shift group
*/

195
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c
View File

@ -1,195 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_shift_q31.c
*
* Description: Shifts the elements of a Q31 vector by a specified number of bits.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup shift Vector Shift
*
* Shifts the elements of a fixed-point vector by a specified number of bits.
* There are separate functions for Q7, Q15, and Q31 data types.
* The underlying algorithm used is:
*
* <pre>
* pDst[n] = pSrc[n] << shift, 0 <= n < blockSize.
* </pre>
*
* If <code>shift</code> is positive then the elements of the vector are shifted to the left.
* If <code>shift</code> is negative then the elements of the vector are shifted to the right.
*/
/**
* @addtogroup shift
* @{
*/
/**
* @brief Shifts the elements of a Q31 vector a specified number of bits.
* @param[in] *pSrc points to the input vector
* @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in the vector
* @return none.
*
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.
*/
void arm_shift_q31(
q31_t * pSrc,
int8_t shiftBits,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */
#ifndef ARM_MATH_CM0
q31_t in1, in2, in3, in4; /* Temporary input variables */
q31_t out1, out2, out3, out4; /* Temporary output variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
if(sign == 0u)
{
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A << shiftBits */
/* Shift the input and then store the results in the destination buffer. */
in1 = *pSrc;
in2 = *(pSrc + 1);
out1 = in1 << shiftBits;
in3 = *(pSrc + 2);
out2 = in2 << shiftBits;
in4 = *(pSrc + 3);
if(in1 != (out1 >> shiftBits))
out1 = 0x7FFFFFFF ^ (in1 >> 31);
if(in2 != (out2 >> shiftBits))
out2 = 0x7FFFFFFF ^ (in2 >> 31);
*pDst = out1;
out3 = in3 << shiftBits;
*(pDst + 1) = out2;
out4 = in4 << shiftBits;
if(in3 != (out3 >> shiftBits))
out3 = 0x7FFFFFFF ^ (in3 >> 31);
if(in4 != (out4 >> shiftBits))
out4 = 0x7FFFFFFF ^ (in4 >> 31);
*(pDst + 2) = out3;
*(pDst + 3) = out4;
/* Update destination pointer to process next sampels */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
}
else
{
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A >> shiftBits */
/* Shift the input and then store the results in the destination buffer. */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
in4 = *(pSrc + 3);
*pDst = (in1 >> -shiftBits);
*(pDst + 1) = (in2 >> -shiftBits);
*(pDst + 2) = (in3 >> -shiftBits);
*(pDst + 3) = (in4 >> -shiftBits);
pSrc += 4u;
pDst += 4u;
blkCnt--;
}
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A (>> or <<) shiftBits */
/* Shift the input and then store the result in the destination buffer. */
*pDst++ = (sign == 0u) ? clip_q63_to_q31((q63_t) * pSrc++ << shiftBits) :
(*pSrc++ >> -shiftBits);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of shift group
*/

Some files were not shown because too many files have changed in this diff Show More