forked from Archive/PX4-Autopilot
Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2
This commit is contained in:
commit
437d9e4180
|
@ -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
|
*.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
|
!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/
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
18
Makefile
18
Makefile
|
@ -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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -95,9 +95,14 @@ all: $(STAGED_FIRMWARES)
|
||||||
#
|
#
|
||||||
# Copy FIRMWARES into the image directory.
|
# 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
|
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
|
||||||
@echo %% Copying $@
|
@echo %% Copying $@
|
||||||
$(Q) $(COPY) $< $@
|
$(Q) $(COPY) $< $@
|
||||||
|
$(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@)
|
||||||
|
|
||||||
#
|
#
|
||||||
# Generate FIRMWARES.
|
# Generate FIRMWARES.
|
||||||
|
@ -140,7 +145,7 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
|
||||||
endif
|
endif
|
||||||
|
|
||||||
$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@))
|
$(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)
|
$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
|
||||||
@echo %% Configuring NuttX for $(board)/$(configuration)
|
@echo %% Configuring NuttX for $(board)/$(configuration)
|
||||||
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
|
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
|
||||||
|
@ -159,11 +164,11 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
|
||||||
.PHONY: clean
|
.PHONY: clean
|
||||||
clean:
|
clean:
|
||||||
$(Q) $(RMDIR) $(BUILD_DIR)*.build
|
$(Q) $(RMDIR) $(BUILD_DIR)*.build
|
||||||
$(Q) $(REMOVE) -f $(IMAGE_DIR)*.px4
|
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
|
||||||
|
|
||||||
.PHONY: distclean
|
.PHONY: distclean
|
||||||
distclean: clean
|
distclean: clean
|
||||||
$(Q) $(REMOVE) -f $(ARCHIVE_DIR)*.export
|
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
|
||||||
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
|
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -196,6 +201,11 @@ help:
|
||||||
@echo " distclean"
|
@echo " distclean"
|
||||||
@echo " Remove all compilation products, including NuttX RTOS archives."
|
@echo " Remove all compilation products, including NuttX RTOS archives."
|
||||||
@echo ""
|
@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 " Common options:"
|
||||||
@echo " ---------------"
|
@echo " ---------------"
|
||||||
@echo ""
|
@echo ""
|
||||||
|
|
|
@ -20,10 +20,10 @@ uorb start
|
||||||
# Load microSD params
|
# Load microSD params
|
||||||
#
|
#
|
||||||
echo "[init] loading microSD params"
|
echo "[init] loading microSD params"
|
||||||
param select /fs/microsd/parameters
|
param select /fs/microsd/params
|
||||||
if [ -f /fs/microsd/parameters ]
|
if [ -f /fs/microsd/params ]
|
||||||
then
|
then
|
||||||
param load /fs/microsd/parameters
|
param load /fs/microsd/params
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -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
|
|
@ -13,10 +13,10 @@ uorb start
|
||||||
# Load microSD params
|
# Load microSD params
|
||||||
#
|
#
|
||||||
echo "[init] loading microSD params"
|
echo "[init] loading microSD params"
|
||||||
param select /fs/microsd/parameters
|
param select /fs/microsd/params
|
||||||
if [ -f /fs/microsd/parameters ]
|
if [ -f /fs/microsd/params ]
|
||||||
then
|
then
|
||||||
param load /fs/microsd/parameters
|
param load /fs/microsd/params
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
|
||||||
uorb start
|
uorb start
|
||||||
|
|
||||||
#
|
#
|
||||||
# Init the parameter storage
|
# Load microSD params
|
||||||
#
|
#
|
||||||
echo "[init] loading microSD params"
|
echo "[init] loading microSD params"
|
||||||
param select /fs/microsd/parameters
|
param select /fs/microsd/params
|
||||||
if [ -f /fs/microsd/parameters ]
|
if [ -f /fs/microsd/params ]
|
||||||
then
|
then
|
||||||
param load /fs/microsd/parameters
|
param load /fs/microsd/params
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -17,10 +17,10 @@ hil mode_pwm
|
||||||
# Load microSD params
|
# Load microSD params
|
||||||
#
|
#
|
||||||
echo "[init] loading microSD params"
|
echo "[init] loading microSD params"
|
||||||
param select /fs/microsd/parameters
|
param select /fs/microsd/params
|
||||||
if [ -f /fs/microsd/parameters ]
|
if [ -f /fs/microsd/params ]
|
||||||
then
|
then
|
||||||
param load /fs/microsd/parameters
|
param load /fs/microsd/params
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -7,6 +7,14 @@
|
||||||
# Start sensor drivers here.
|
# Start sensor drivers here.
|
||||||
#
|
#
|
||||||
|
|
||||||
|
#
|
||||||
|
# Check for UORB
|
||||||
|
#
|
||||||
|
if uorb start
|
||||||
|
then
|
||||||
|
echo "uORB started"
|
||||||
|
fi
|
||||||
|
|
||||||
ms5611 start
|
ms5611 start
|
||||||
adc start
|
adc start
|
||||||
|
|
||||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -0,0 +1,10 @@
|
||||||
|
*.a
|
||||||
|
*.bdat
|
||||||
|
*.pdat
|
||||||
|
.built
|
||||||
|
.config
|
||||||
|
.depend
|
||||||
|
.updated
|
||||||
|
builtin/builtin_list.h
|
||||||
|
builtin/builtin_proto.h
|
||||||
|
Make.dep
|
|
@ -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.
|
|
@ -14,6 +14,7 @@ MODULES += drivers/device
|
||||||
MODULES += drivers/stm32
|
MODULES += drivers/stm32
|
||||||
MODULES += drivers/stm32/adc
|
MODULES += drivers/stm32/adc
|
||||||
MODULES += drivers/stm32/tone_alarm
|
MODULES += drivers/stm32/tone_alarm
|
||||||
|
MODULES += drivers/led
|
||||||
MODULES += drivers/px4io
|
MODULES += drivers/px4io
|
||||||
MODULES += drivers/px4fmu
|
MODULES += drivers/px4fmu
|
||||||
MODULES += drivers/boards/px4fmu
|
MODULES += drivers/boards/px4fmu
|
||||||
|
@ -29,6 +30,9 @@ MODULES += drivers/gps
|
||||||
MODULES += drivers/hil
|
MODULES += drivers/hil
|
||||||
MODULES += drivers/hott_telemetry
|
MODULES += drivers/hott_telemetry
|
||||||
MODULES += drivers/blinkm
|
MODULES += drivers/blinkm
|
||||||
|
MODULES += drivers/mkblctrl
|
||||||
|
MODULES += drivers/md25
|
||||||
|
MODULES += drivers/ets_airspeed
|
||||||
MODULES += modules/sensors
|
MODULES += modules/sensors
|
||||||
|
|
||||||
#
|
#
|
||||||
|
@ -77,15 +81,19 @@ MODULES += modules/multirotor_pos_control
|
||||||
MODULES += modules/sdlog
|
MODULES += modules/sdlog
|
||||||
|
|
||||||
#
|
#
|
||||||
# Libraries
|
# Library modules
|
||||||
#
|
#
|
||||||
MODULES += modules/systemlib
|
MODULES += modules/systemlib
|
||||||
MODULES += modules/systemlib/mixer
|
MODULES += modules/systemlib/mixer
|
||||||
MODULES += modules/mathlib
|
MODULES += modules/mathlib
|
||||||
MODULES += modules/mathlib/CMSIS
|
|
||||||
MODULES += modules/controllib
|
MODULES += modules/controllib
|
||||||
MODULES += modules/uORB
|
MODULES += modules/uORB
|
||||||
|
|
||||||
|
#
|
||||||
|
# Libraries
|
||||||
|
#
|
||||||
|
LIBRARIES += modules/mathlib/CMSIS
|
||||||
|
|
||||||
#
|
#
|
||||||
# Demo apps
|
# Demo apps
|
||||||
#
|
#
|
||||||
|
@ -102,6 +110,10 @@ MODULES += modules/uORB
|
||||||
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
# https://pixhawk.ethz.ch/px4/dev/debug_values
|
||||||
#MODULES += examples/px4_mavlink_debug
|
#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.
|
# Transitional support - add commands from the NuttX export archive.
|
||||||
#
|
#
|
||||||
|
|
|
@ -74,15 +74,19 @@ MODULES += modules/multirotor_pos_control
|
||||||
MODULES += modules/sdlog
|
MODULES += modules/sdlog
|
||||||
|
|
||||||
#
|
#
|
||||||
# Libraries
|
# Library modules
|
||||||
#
|
#
|
||||||
MODULES += modules/systemlib
|
MODULES += modules/systemlib
|
||||||
MODULES += modules/systemlib/mixer
|
MODULES += modules/systemlib/mixer
|
||||||
MODULES += modules/mathlib
|
MODULES += modules/mathlib
|
||||||
MODULES += modules/mathlib/CMSIS
|
|
||||||
MODULES += modules/controllib
|
MODULES += modules/controllib
|
||||||
MODULES += modules/uORB
|
MODULES += modules/uORB
|
||||||
|
|
||||||
|
#
|
||||||
|
# Libraries
|
||||||
|
#
|
||||||
|
LIBRARIES += modules/mathlib/CMSIS
|
||||||
|
|
||||||
#
|
#
|
||||||
# Demo apps
|
# Demo apps
|
||||||
#
|
#
|
||||||
|
|
|
@ -180,20 +180,8 @@ EXTRA_CLEANS =
|
||||||
# Modules
|
# 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
|
# 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
|
# sort and unique the modules list
|
||||||
MODULES := $(sort $(MODULES))
|
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
|
# locate the first instance of a module by full path or by looking on the
|
||||||
# module search path
|
# module search path
|
||||||
define MODULE_SEARCH
|
define MODULE_SEARCH
|
||||||
$(abspath $(firstword $(wildcard $(1)/module.mk) \
|
$(firstword $(abspath $(wildcard $(1)/module.mk)) \
|
||||||
$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
|
$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
|
||||||
MISSING_$1))
|
MISSING_$1)
|
||||||
endef
|
endef
|
||||||
|
|
||||||
# make a list of module makefiles and check that we found them all
|
# 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)
|
.PHONY: $(MODULE_OBJS)
|
||||||
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
|
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
|
||||||
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
|
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
|
||||||
|
$(MODULE_OBJS): workdir = $(@D)
|
||||||
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
|
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
|
||||||
|
$(Q) $(MKDIR) -p $(workdir)
|
||||||
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
|
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
|
||||||
MODULE_WORK_DIR=$(dir $@) \
|
-C $(workdir) \
|
||||||
|
MODULE_WORK_DIR=$(workdir) \
|
||||||
MODULE_OBJ=$@ \
|
MODULE_OBJ=$@ \
|
||||||
MODULE_MK=$(mkfile) \
|
MODULE_MK=$(mkfile) \
|
||||||
MODULE_NAME=$(lastword $(subst /, ,$(@D))) \
|
MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
|
||||||
module
|
module
|
||||||
|
|
||||||
# make a list of phony clean targets for modules
|
# make a list of phony clean targets for modules
|
||||||
|
@ -245,6 +236,66 @@ $(MODULE_CLEANS):
|
||||||
MODULE_MK=$(mkfile) \
|
MODULE_MK=$(mkfile) \
|
||||||
clean
|
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
|
# NuttX libraries and paths
|
||||||
################################################################################
|
################################################################################
|
||||||
|
@ -266,14 +317,18 @@ endif
|
||||||
#
|
#
|
||||||
|
|
||||||
# Add dependencies on anything in the ROMFS root
|
# Add dependencies on anything in the ROMFS root
|
||||||
ROMFS_DEPS += $(wildcard \
|
ROMFS_FILES += $(wildcard \
|
||||||
(ROMFS_ROOT)/* \
|
$(ROMFS_ROOT)/* \
|
||||||
(ROMFS_ROOT)/*/* \
|
$(ROMFS_ROOT)/*/* \
|
||||||
(ROMFS_ROOT)/*/*/* \
|
$(ROMFS_ROOT)/*/*/* \
|
||||||
(ROMFS_ROOT)/*/*/*/* \
|
$(ROMFS_ROOT)/*/*/*/* \
|
||||||
(ROMFS_ROOT)/*/*/*/*/* \
|
$(ROMFS_ROOT)/*/*/*/*/* \
|
||||||
(ROMFS_ROOT)/*/*/*/*/*/*)
|
$(ROMFS_ROOT)/*/*/*/*/*/*)
|
||||||
ROMFS_IMG = $(WORK_DIR)romfs.img
|
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_CSRC = $(ROMFS_IMG:.img=.c)
|
||||||
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
|
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
|
||||||
LIBS += $(ROMFS_OBJ)
|
LIBS += $(ROMFS_OBJ)
|
||||||
|
@ -413,8 +468,8 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN)
|
||||||
$(PRODUCT_BIN): $(PRODUCT_ELF)
|
$(PRODUCT_BIN): $(PRODUCT_ELF)
|
||||||
$(call SYM_TO_BIN,$<,$@)
|
$(call SYM_TO_BIN,$<,$@)
|
||||||
|
|
||||||
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
|
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
|
||||||
$(call LINK,$@,$(OBJS) $(MODULE_OBJS))
|
$(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS))
|
||||||
|
|
||||||
#
|
#
|
||||||
# Utility rules
|
# Utility rules
|
||||||
|
|
|
@ -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)
|
|
@ -35,10 +35,14 @@
|
||||||
# This makefile is invoked by firmware.mk to build each of the modules
|
# This makefile is invoked by firmware.mk to build each of the modules
|
||||||
# that will subsequently be linked into the firmware image.
|
# 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
|
# symbols, as the global namespace is shared between all modules. Normally an
|
||||||
# module will just export one or more <command>_main functions.
|
# 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:
|
# 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)
|
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
|
||||||
|
|
||||||
#
|
##
|
||||||
# Locate sources (allows relative source paths in module.mk)
|
## Object files we will generate from sources
|
||||||
#
|
##
|
||||||
define SRC_SEARCH
|
OBJS = $(addsuffix .o,$(SRCS))
|
||||||
$(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)
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# SRCS -> OBJS rules
|
# SRCS -> OBJS rules
|
||||||
|
@ -206,13 +194,16 @@ OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
|
||||||
|
|
||||||
$(OBJS): $(GLOBAL_DEPS)
|
$(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,$<,$@)
|
$(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,$<,$@)
|
$(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,$<,$@)
|
$(call ASSEMBLE,$<,$@)
|
||||||
|
|
||||||
#
|
#
|
||||||
|
|
|
@ -144,6 +144,7 @@ CFLAGS = $(ARCHCFLAGS) \
|
||||||
$(INSTRUMENTATIONDEFINES) \
|
$(INSTRUMENTATIONDEFINES) \
|
||||||
$(ARCHDEFINES) \
|
$(ARCHDEFINES) \
|
||||||
$(EXTRADEFINES) \
|
$(EXTRADEFINES) \
|
||||||
|
$(EXTRACFLAGS) \
|
||||||
-fno-common \
|
-fno-common \
|
||||||
$(addprefix -I,$(INCLUDE_DIRS))
|
$(addprefix -I,$(INCLUDE_DIRS))
|
||||||
|
|
||||||
|
@ -156,18 +157,22 @@ CXXFLAGS = $(ARCHCXXFLAGS) \
|
||||||
$(ARCHXXINCLUDES) \
|
$(ARCHXXINCLUDES) \
|
||||||
$(INSTRUMENTATIONDEFINES) \
|
$(INSTRUMENTATIONDEFINES) \
|
||||||
$(ARCHDEFINES) \
|
$(ARCHDEFINES) \
|
||||||
$(EXTRADEFINES) \
|
|
||||||
-DCONFIG_WCHAR_BUILTIN \
|
-DCONFIG_WCHAR_BUILTIN \
|
||||||
|
$(EXTRADEFINES) \
|
||||||
|
$(EXTRACXXFLAGS) \
|
||||||
$(addprefix -I,$(INCLUDE_DIRS))
|
$(addprefix -I,$(INCLUDE_DIRS))
|
||||||
|
|
||||||
# Flags we pass to the assembler
|
# Flags we pass to the assembler
|
||||||
#
|
#
|
||||||
AFLAGS = $(CFLAGS) -D__ASSEMBLY__
|
AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
|
||||||
|
$(EXTRADEFINES) \
|
||||||
|
$(EXTRAAFLAGS)
|
||||||
|
|
||||||
# Flags we pass to the linker
|
# Flags we pass to the linker
|
||||||
#
|
#
|
||||||
LDFLAGS += --warn-common \
|
LDFLAGS += --warn-common \
|
||||||
--gc-sections \
|
--gc-sections \
|
||||||
|
$(EXTRALDFLAGS) \
|
||||||
$(addprefix -T,$(LDSCRIPT)) \
|
$(addprefix -T,$(LDSCRIPT)) \
|
||||||
$(addprefix -L,$(LIB_DIRS))
|
$(addprefix -L,$(LIB_DIRS))
|
||||||
|
|
||||||
|
@ -249,6 +254,20 @@ endef
|
||||||
# - relink the object and insert the binary file
|
# - relink the object and insert the binary file
|
||||||
# - edit symbol names to suit
|
# - 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
|
define BIN_SYM_PREFIX
|
||||||
_binary_$(subst /,_,$(subst .,_,$1))
|
_binary_$(subst /,_,$(subst .,_,$1))
|
||||||
endef
|
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)_start=$3 \
|
||||||
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
|
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
|
||||||
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
|
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
|
||||||
|
$(Q) $(REMOVE) $2.c $2.c.o
|
||||||
endef
|
endef
|
||||||
|
|
|
@ -30,9 +30,6 @@ upload-serial-px4fmu: $(BUNDLE) $(UPLOADER)
|
||||||
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
|
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
|
||||||
$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
|
$(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
|
# JTAG firmware uploading with OpenOCD
|
||||||
#
|
#
|
||||||
|
|
|
@ -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
|
|
@ -1512,7 +1512,6 @@ static inline void stm32_ep0out_receive(FAR struct stm32_ep_s *privep, int bcnt)
|
||||||
|
|
||||||
DEBUGASSERT(privep && privep->ep.priv);
|
DEBUGASSERT(privep && privep->ep.priv);
|
||||||
priv = (FAR struct stm32_usbdev_s *)privep->ep.priv;
|
priv = (FAR struct stm32_usbdev_s *)privep->ep.priv;
|
||||||
DEBUGASSERT(priv->ep0state == EP0STATE_SETUP_OUT);
|
|
||||||
|
|
||||||
ullvdbg("EP0: bcnt=%d\n", bcnt);
|
ullvdbg("EP0: bcnt=%d\n", bcnt);
|
||||||
usbtrace(TRACE_READ(EP0), bcnt);
|
usbtrace(TRACE_READ(EP0), bcnt);
|
||||||
|
|
|
@ -647,10 +647,14 @@ CONFIG_DISABLE_POLL=n
|
||||||
# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
|
# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
|
||||||
# 5.1234567
|
# 5.1234567
|
||||||
# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu)
|
# 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_NOPRINTF_FIELDWIDTH=n
|
||||||
CONFIG_LIBC_FLOATINGPOINT=y
|
CONFIG_LIBC_FLOATINGPOINT=y
|
||||||
CONFIG_HAVE_LONG_LONG=y
|
CONFIG_HAVE_LONG_LONG=y
|
||||||
|
CONFIG_LIBC_STRERROR=n
|
||||||
|
CONFIG_LIBC_STRERROR_SHORT=n
|
||||||
|
|
||||||
#
|
#
|
||||||
# Allow for architecture optimized implementations
|
# Allow for architecture optimized implementations
|
||||||
|
|
|
@ -43,6 +43,7 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Included Files
|
* Included Files
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Pre-processor Definitions
|
* Pre-processor Definitions
|
||||||
|
@ -104,6 +105,13 @@ static void zeroes(FAR struct lib_outstream_s *obj, int nzeroes)
|
||||||
* Private Functions
|
* 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
|
* Name: lib_dtoa
|
||||||
*
|
*
|
||||||
|
@ -137,9 +145,23 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
|
||||||
int nchars; /* Number of characters to print */
|
int nchars; /* Number of characters to print */
|
||||||
int dsgn; /* Unused sign indicator */
|
int dsgn; /* Unused sign indicator */
|
||||||
int i;
|
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 */
|
/* Non-zero... positive or negative */
|
||||||
|
|
||||||
if (value < 0)
|
if (value < 0)
|
||||||
{
|
{
|
||||||
value = -value;
|
value = -value;
|
||||||
|
@ -178,6 +200,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
|
||||||
if (prec > 0 || IS_ALTFORM(flags))
|
if (prec > 0 || IS_ALTFORM(flags))
|
||||||
{
|
{
|
||||||
obj->put(obj, '.');
|
obj->put(obj, '.');
|
||||||
|
done_decimal_point = true;
|
||||||
|
|
||||||
/* Always print at least one digit to the right of the decimal point. */
|
/* 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 */
|
/* Print the decimal point */
|
||||||
|
|
||||||
obj->put(obj, '.');
|
obj->put(obj, '.');
|
||||||
|
done_decimal_point = true;
|
||||||
|
|
||||||
/* Print any leading zeros to the right of the decimal point */
|
/* 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 */
|
/* Print the decimal point */
|
||||||
|
|
||||||
obj->put(obj, '.');
|
obj->put(obj, '.');
|
||||||
|
done_decimal_point = true;
|
||||||
|
|
||||||
/* Always print at least one digit to the right of the decimal
|
/* Always print at least one digit to the right of the decimal
|
||||||
* point.
|
* point.
|
||||||
|
@ -285,8 +310,9 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Finally, print any trailing zeroes */
|
/* Finally, print any trailing zeroes */
|
||||||
|
if (done_decimal_point) {
|
||||||
zeroes(obj, prec);
|
zeroes(obj, prec);
|
||||||
|
}
|
||||||
|
|
||||||
/* Is this memory supposed to be freed or not? */
|
/* Is this memory supposed to be freed or not? */
|
||||||
|
|
||||||
|
|
|
@ -1215,7 +1215,7 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
|
||||||
fmt = FMT_RJUST;
|
fmt = FMT_RJUST;
|
||||||
width = 0;
|
width = 0;
|
||||||
#ifdef CONFIG_LIBC_FLOATINGPOINT
|
#ifdef CONFIG_LIBC_FLOATINGPOINT
|
||||||
trunc = 0;
|
trunc = 6;
|
||||||
#endif
|
#endif
|
||||||
#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
|
#ifndef CONFIG_NOPRINTF_FIELDWIDTH
|
||||||
fmt = FMT_RJUST0;
|
fmt = FMT_RJUST0;
|
||||||
|
#ifdef CONFIG_LIBC_FLOATINGPOINT
|
||||||
|
if (IS_HASDOT(flags)) {
|
||||||
|
trunc = 0;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
#if 0
|
#if 0
|
||||||
|
|
|
@ -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;
|
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
|
||||||
|
|
||||||
/* Failsafe logic - should never be necessary */
|
/* Failsafe logic - should never be necessary */
|
||||||
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
|
motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
|
||||||
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
|
motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
|
||||||
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
|
motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
|
||||||
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
|
motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
|
||||||
|
|
||||||
/* send motors via UART */
|
/* send motors via UART */
|
||||||
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
|
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
|
||||||
|
|
|
@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \
|
||||||
px4fmu_init.c \
|
px4fmu_init.c \
|
||||||
px4fmu_pwm_servo.c \
|
px4fmu_pwm_servo.c \
|
||||||
px4fmu_spi.c \
|
px4fmu_spi.c \
|
||||||
px4fmu_usb.c
|
px4fmu_usb.c \
|
||||||
|
px4fmu_led.c
|
||||||
|
|
|
@ -1,6 +1,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
*
|
*
|
||||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -91,6 +91,19 @@
|
||||||
# endif
|
# endif
|
||||||
#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
|
* Protected Functions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void)
|
||||||
/* configure SPI interfaces */
|
/* configure SPI interfaces */
|
||||||
stm32_spiinitialize();
|
stm32_spiinitialize();
|
||||||
|
|
||||||
/* configure LEDs */
|
/* configure LEDs (empty call to NuttX' ledinit) */
|
||||||
up_ledinit();
|
up_ledinit();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -178,11 +191,11 @@ __EXPORT int nsh_archinitialize(void)
|
||||||
(hrt_callout)stm32_serial_dma_poll,
|
(hrt_callout)stm32_serial_dma_poll,
|
||||||
NULL);
|
NULL);
|
||||||
|
|
||||||
// initial LED state
|
/* initial LED state */
|
||||||
// drv_led_start();
|
drv_led_start();
|
||||||
up_ledoff(LED_BLUE);
|
led_off(LED_AMBER);
|
||||||
up_ledoff(LED_AMBER);
|
led_on(LED_BLUE);
|
||||||
up_ledon(LED_BLUE);
|
|
||||||
|
|
||||||
/* Configure SPI-based devices */
|
/* Configure SPI-based devices */
|
||||||
|
|
||||||
|
|
|
@ -39,19 +39,27 @@
|
||||||
|
|
||||||
#include <nuttx/config.h>
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.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 "stm32_internal.h"
|
||||||
#include "px4fmu_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 */
|
/* Configure LED1-2 GPIOs for output */
|
||||||
|
|
||||||
|
@ -59,7 +67,7 @@ __EXPORT void up_ledinit()
|
||||||
stm32_configgpio(GPIO_LED2);
|
stm32_configgpio(GPIO_LED2);
|
||||||
}
|
}
|
||||||
|
|
||||||
__EXPORT void up_ledon(int led)
|
__EXPORT void led_on(int led)
|
||||||
{
|
{
|
||||||
if (led == 0)
|
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)
|
if (led == 0)
|
||||||
{
|
{
|
||||||
|
|
|
@ -53,6 +53,15 @@ namespace device __EXPORT
|
||||||
class __EXPORT I2C : public CDev
|
class __EXPORT I2C : public CDev
|
||||||
{
|
{
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Get the address
|
||||||
|
*/
|
||||||
|
uint16_t get_address() {
|
||||||
|
return _address;
|
||||||
|
}
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
/**
|
/**
|
||||||
* The number of times a read or write operation will be retried on
|
* The number of times a read or write operation will be retried on
|
||||||
|
|
|
@ -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 */
|
|
@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
|
||||||
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
|
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
|
||||||
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
|
#define PWM_SERVO_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 */
|
/** set a single servo to a specific value */
|
||||||
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
|
||||||
|
|
||||||
|
|
|
@ -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
30
src/modules/attitude_estimator_ekf/Makefile → src/drivers/ets_airspeed/module.mk
Executable file → Normal 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
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -31,27 +31,11 @@
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
APPNAME = attitude_estimator_ekf
|
#
|
||||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
# Makefile to build the Eagle Tree Airspeed V3 driver.
|
||||||
STACKSIZE = 2048
|
#
|
||||||
|
|
||||||
CXXSRCS = attitude_estimator_ekf_main.cpp
|
MODULE_COMMAND = ets_airspeed
|
||||||
|
MODULE_STACKSIZE = 1024
|
||||||
|
|
||||||
CSRCS = attitude_estimator_ekf_params.c \
|
SRCS = ets_airspeed.cpp
|
||||||
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
|
|
|
@ -285,6 +285,10 @@ GPS::task_main()
|
||||||
unlock();
|
unlock();
|
||||||
if (_Helper->configure(_baudrate) == 0) {
|
if (_Helper->configure(_baudrate) == 0) {
|
||||||
unlock();
|
unlock();
|
||||||
|
|
||||||
|
// GPS is obviously detected successfully, reset statistics
|
||||||
|
_Helper->reset_update_rates();
|
||||||
|
|
||||||
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
|
||||||
// lock();
|
// lock();
|
||||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
/* 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);
|
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
|
||||||
last_rate_measurement = hrt_absolute_time();
|
last_rate_measurement = hrt_absolute_time();
|
||||||
last_rate_count = 0;
|
last_rate_count = 0;
|
||||||
|
_Helper->store_update_rates();
|
||||||
|
_Helper->reset_update_rates();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_healthy) {
|
if (!_healthy) {
|
||||||
|
@ -372,7 +378,10 @@ GPS::print_info()
|
||||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||||
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
||||||
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
|
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);
|
usleep(100000);
|
||||||
|
|
|
@ -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>
|
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
* Julian Oes <joes@student.ethz.ch>
|
* Julian Oes <joes@student.ethz.ch>
|
||||||
*
|
*
|
||||||
|
@ -36,9 +36,39 @@
|
||||||
#include <termios.h>
|
#include <termios.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
#include "gps_helper.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
|
int
|
||||||
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||||
|
|
|
@ -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>
|
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||||
* Julian Oes <joes@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
|
#ifndef GPS_HELPER_H
|
||||||
#define GPS_HELPER_H
|
#define GPS_HELPER_H
|
||||||
|
@ -44,9 +46,22 @@
|
||||||
class GPS_Helper
|
class GPS_Helper
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
virtual int configure(unsigned &baud) = 0;
|
virtual int configure(unsigned &baud) = 0;
|
||||||
virtual int receive(unsigned timeout) = 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 */
|
#endif /* GPS_HELPER_H */
|
||||||
|
|
|
@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
|
||||||
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
|
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
|
||||||
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
|
_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;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -87,14 +87,14 @@ class MTK : public GPS_Helper
|
||||||
public:
|
public:
|
||||||
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
|
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||||
~MTK();
|
~MTK();
|
||||||
int receive(unsigned timeout);
|
int receive(unsigned timeout);
|
||||||
int configure(unsigned &baudrate);
|
int configure(unsigned &baudrate);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/**
|
/**
|
||||||
* Parse the binary MTK packet
|
* 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
|
* Handle the package once it has arrived
|
||||||
|
|
|
@ -60,7 +60,8 @@
|
||||||
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||||
_fd(fd),
|
_fd(fd),
|
||||||
_gps_position(gps_position),
|
_gps_position(gps_position),
|
||||||
_waiting_for_ack(false)
|
_waiting_for_ack(false),
|
||||||
|
_disable_cmd_counter(0)
|
||||||
{
|
{
|
||||||
decode_init();
|
decode_init();
|
||||||
}
|
}
|
||||||
|
@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
|
||||||
cfg_rate_packet.clsID = UBX_CLASS_CFG;
|
cfg_rate_packet.clsID = UBX_CLASS_CFG;
|
||||||
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
|
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
|
||||||
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
|
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.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
|
||||||
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
|
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
|
||||||
|
|
||||||
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
|
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 */
|
/* try next baudrate */
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
|
||||||
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
|
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
|
||||||
|
|
||||||
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
|
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 */
|
/* try next baudrate */
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
|
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
|
||||||
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
|
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
|
||||||
|
/* insist of receiving the ACK for this packet */
|
||||||
_clsID_needed = UBX_CLASS_CFG;
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
_msgID_needed = UBX_MESSAGE_CFG_MSG;
|
// continue;
|
||||||
|
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
|
||||||
cfg_msg_packet.clsID = UBX_CLASS_CFG;
|
1);
|
||||||
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
|
// /* insist of receiving the ACK for this packet */
|
||||||
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
|
// continue;
|
||||||
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
|
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
|
||||||
|
1);
|
||||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
// /* insist of receiving the ACK for this packet */
|
||||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
|
// continue;
|
||||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
|
||||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
1);
|
||||||
/* try next baudrate */
|
// /* insist of receiving the ACK for this packet */
|
||||||
continue;
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
}
|
// continue;
|
||||||
|
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
|
||||||
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
|
// 0);
|
||||||
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
|
// /* insist of receiving the ACK for this packet */
|
||||||
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
|
// continue;
|
||||||
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
|
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
|
||||||
/* try next baudrate */
|
0);
|
||||||
continue;
|
// /* insist of receiving the ACK for this packet */
|
||||||
}
|
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
|
||||||
|
// 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;
|
|
||||||
|
|
||||||
_waiting_for_ack = false;
|
_waiting_for_ack = false;
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int
|
||||||
|
UBX::wait_for_ack(unsigned timeout)
|
||||||
|
{
|
||||||
|
_waiting_for_ack = true;
|
||||||
|
int ret = receive(timeout);
|
||||||
|
_waiting_for_ack = false;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
UBX::receive(unsigned timeout)
|
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->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
|
||||||
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
|
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
|
||||||
|
|
||||||
|
_rate_count_lat_lon++;
|
||||||
|
|
||||||
/* Add timestamp to finish the report */
|
/* Add timestamp to finish the report */
|
||||||
_gps_position->timestamp_position = hrt_absolute_time();
|
_gps_position->timestamp_position = hrt_absolute_time();
|
||||||
/* only return 1 when new position is available */
|
/* 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->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
|
||||||
_gps_position->vel_ned_valid = true;
|
_gps_position->vel_ned_valid = true;
|
||||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||||
|
|
||||||
|
_rate_count_vel++;
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
@ -693,6 +674,12 @@ UBX::handle_message()
|
||||||
|
|
||||||
default: //we don't know the message
|
default: //we don't know the message
|
||||||
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
|
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;
|
ret = -1;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
|
||||||
message[length-1] = ck_b;
|
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
|
void
|
||||||
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
|
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?
|
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
|
||||||
warnx("ubx: config write fail");
|
warnx("ubx: 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);
|
||||||
|
}
|
||||||
|
|
|
@ -65,26 +65,27 @@
|
||||||
#define UBX_MESSAGE_CFG_RATE 0x08
|
#define UBX_MESSAGE_CFG_RATE 0x08
|
||||||
|
|
||||||
#define UBX_CFG_PRT_LENGTH 20
|
#define UBX_CFG_PRT_LENGTH 20
|
||||||
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
|
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
|
||||||
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||||
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
|
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
|
||||||
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
|
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
|
||||||
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
|
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
|
||||||
|
|
||||||
#define UBX_CFG_RATE_LENGTH 6
|
#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_NAVRATE 1 /**< cannot be changed */
|
||||||
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
|
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
|
||||||
|
|
||||||
|
|
||||||
#define UBX_CFG_NAV5_LENGTH 36
|
#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_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_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_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_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
|
#define UBX_MAX_PAYLOAD_LENGTH 500
|
||||||
|
|
||||||
|
@ -92,6 +93,14 @@
|
||||||
/** the structures of the binary packets */
|
/** the structures of the binary packets */
|
||||||
#pragma pack(push, 1)
|
#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 {
|
typedef struct {
|
||||||
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
|
||||||
int32_t lon; /**< Longitude * 1e-7, deg */
|
int32_t lon; /**< Longitude * 1e-7, deg */
|
||||||
|
@ -274,11 +283,17 @@ typedef struct {
|
||||||
uint16_t length;
|
uint16_t length;
|
||||||
uint8_t msgClass_payload;
|
uint8_t msgClass_payload;
|
||||||
uint8_t msgID_payload;
|
uint8_t msgID_payload;
|
||||||
uint8_t rate[6];
|
uint8_t rate;
|
||||||
uint8_t ck_a;
|
uint8_t ck_a;
|
||||||
uint8_t ck_b;
|
uint8_t ck_b;
|
||||||
} type_gps_bin_cfg_msg_packet_t;
|
} 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
|
// END the structures of the binary packets
|
||||||
// ************
|
// ************
|
||||||
|
@ -341,55 +356,64 @@ class UBX : public GPS_Helper
|
||||||
public:
|
public:
|
||||||
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
|
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
|
||||||
~UBX();
|
~UBX();
|
||||||
int receive(unsigned timeout);
|
int receive(unsigned timeout);
|
||||||
int configure(unsigned &baudrate);
|
int configure(unsigned &baudrate);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Parse the binary MTK packet
|
* Parse the binary MTK packet
|
||||||
*/
|
*/
|
||||||
int parse_char(uint8_t b);
|
int parse_char(uint8_t b);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Handle the package once it has arrived
|
* Handle the package once it has arrived
|
||||||
*/
|
*/
|
||||||
int handle_message(void);
|
int handle_message(void);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Reset the parse state machine for a fresh start
|
* 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
|
* 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
|
* 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
|
* 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;
|
struct vehicle_gps_position_s *_gps_position;
|
||||||
ubx_config_state_t _config_state;
|
ubx_config_state_t _config_state;
|
||||||
bool _waiting_for_ack;
|
bool _waiting_for_ack;
|
||||||
uint8_t _clsID_needed;
|
uint8_t _clsID_needed;
|
||||||
uint8_t _msgID_needed;
|
uint8_t _msgID_needed;
|
||||||
ubx_decode_state_t _decode_state;
|
ubx_decode_state_t _decode_state;
|
||||||
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
|
||||||
unsigned _rx_count;
|
unsigned _rx_count;
|
||||||
uint8_t _rx_ck_a;
|
uint8_t _rx_ck_a;
|
||||||
uint8_t _rx_ck_b;
|
uint8_t _rx_ck_b;
|
||||||
ubx_message_class_t _message_class;
|
ubx_message_class_t _message_class;
|
||||||
ubx_message_id_t _message_id;
|
ubx_message_id_t _message_id;
|
||||||
unsigned _payload_size;
|
unsigned _payload_size;
|
||||||
|
uint8_t _disable_cmd_counter;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif /* UBX_H_ */
|
#endif /* UBX_H_ */
|
||||||
|
|
|
@ -1225,19 +1225,25 @@ start()
|
||||||
|
|
||||||
/* create the driver, attempt expansion bus first */
|
/* create the driver, attempt expansion bus first */
|
||||||
g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
|
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
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
/* if this failed, attempt onboard sensor */
|
/* if this failed, attempt onboard sensor */
|
||||||
if (g_dev == nullptr)
|
if (g_dev == nullptr) {
|
||||||
g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
|
g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
|
||||||
|
if (g_dev != nullptr && OK != g_dev->init()) {
|
||||||
|
goto fail;
|
||||||
|
}
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (g_dev == nullptr)
|
if (g_dev == nullptr)
|
||||||
goto fail;
|
goto fail;
|
||||||
|
|
||||||
if (OK != g_dev->init())
|
|
||||||
goto fail;
|
|
||||||
|
|
||||||
/* set the poll rate to default, starts automatic data collection */
|
/* set the poll rate to default, starts automatic data collection */
|
||||||
fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||||
|
|
||||||
|
|
|
@ -42,9 +42,11 @@
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <systemlib/systemlib.h>
|
#include <systemlib/systemlib.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
#include <uORB/topics/airspeed.h>
|
||||||
#include <uORB/topics/battery_status.h>
|
#include <uORB/topics/battery_status.h>
|
||||||
#include <uORB/topics/sensor_combined.h>
|
#include <uORB/topics/sensor_combined.h>
|
||||||
|
|
||||||
|
static int airspeed_sub = -1;
|
||||||
static int battery_sub = -1;
|
static int battery_sub = -1;
|
||||||
static int sensor_sub = -1;
|
static int sensor_sub = -1;
|
||||||
|
|
||||||
|
@ -52,6 +54,7 @@ void messages_init(void)
|
||||||
{
|
{
|
||||||
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||||
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||||
|
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||||
}
|
}
|
||||||
|
|
||||||
void build_eam_response(uint8_t *buffer, int *size)
|
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_L = (uint8_t)alt & 0xff;
|
||||||
msg.altitude_H = (uint8_t)(alt >> 8) & 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;
|
msg.stop = STOP_BYTE;
|
||||||
|
|
||||||
memcpy(buffer, &msg, *size);
|
memcpy(buffer, &msg, *size);
|
||||||
|
|
|
@ -41,12 +41,17 @@
|
||||||
#include <drivers/device/device.h>
|
#include <drivers/device/device.h>
|
||||||
#include <drivers/drv_led.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
|
__BEGIN_DECLS
|
||||||
extern void up_ledinit();
|
extern void led_init();
|
||||||
extern void up_ledon(int led);
|
extern void led_on(int led);
|
||||||
extern void up_ledoff(int led);
|
extern void led_off(int led);
|
||||||
__END_DECLS
|
__END_DECLS
|
||||||
|
|
||||||
class LED : device::CDev
|
class LED : device::CDev
|
||||||
|
@ -74,7 +79,7 @@ int
|
||||||
LED::init()
|
LED::init()
|
||||||
{
|
{
|
||||||
CDev::init();
|
CDev::init();
|
||||||
up_ledinit();
|
led_init();
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||||
|
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
case LED_ON:
|
case LED_ON:
|
||||||
up_ledon(arg);
|
led_on(arg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case LED_OFF:
|
case LED_OFF:
|
||||||
up_ledoff(arg);
|
led_off(arg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
File diff suppressed because it is too large
Load Diff
|
@ -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
|
|
@ -574,7 +574,11 @@ PX4FMU::task_main()
|
||||||
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
|
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
|
||||||
|
|
||||||
/* update PWM servo armed status if armed and not locked down */
|
/* 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
|
#ifdef FMU_HAVE_PPM
|
||||||
|
@ -675,6 +679,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
up_pwm_servo_arm(true);
|
up_pwm_servo_arm(true);
|
||||||
break;
|
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:
|
case PWM_SERVO_DISARM:
|
||||||
up_pwm_servo_arm(false);
|
up_pwm_servo_arm(false);
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -108,6 +108,14 @@ public:
|
||||||
*/
|
*/
|
||||||
int set_update_rate(int rate);
|
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
|
* Print the current status of IO
|
||||||
*/
|
*/
|
||||||
|
@ -151,6 +159,10 @@ private:
|
||||||
|
|
||||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
bool _primary_pwm_device; ///< true if we are the default PWM output
|
||||||
|
|
||||||
|
float _battery_amp_per_volt;
|
||||||
|
float _battery_amp_bias;
|
||||||
|
float _battery_mamphour_total;
|
||||||
|
uint64_t _battery_last_timestamp;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Trampoline to the worker task
|
* Trampoline to the worker task
|
||||||
|
@ -314,6 +326,10 @@ PX4IO::PX4IO() :
|
||||||
_to_actuators_effective(0),
|
_to_actuators_effective(0),
|
||||||
_to_outputs(0),
|
_to_outputs(0),
|
||||||
_to_battery(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)
|
_primary_pwm_device(false)
|
||||||
{
|
{
|
||||||
/* we need this potentially before it could be set in task_main */
|
/* 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
|
* already armed, and has been configured for in-air restart
|
||||||
*/
|
*/
|
||||||
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
|
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");
|
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
|
||||||
|
|
||||||
|
@ -484,10 +500,9 @@ PX4IO::init()
|
||||||
|
|
||||||
/* dis-arm IO before touching anything */
|
/* dis-arm IO before touching anything */
|
||||||
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
|
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_INAIR_RESTART_OK |
|
||||||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
|
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
|
||||||
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
|
|
||||||
|
|
||||||
/* publish RC config to IO */
|
/* publish RC config to IO */
|
||||||
ret = io_set_rc_config();
|
ret = io_set_rc_config();
|
||||||
|
@ -686,16 +701,18 @@ PX4IO::io_set_arming_state()
|
||||||
uint16_t set = 0;
|
uint16_t set = 0;
|
||||||
uint16_t clear = 0;
|
uint16_t clear = 0;
|
||||||
|
|
||||||
if (armed.armed) {
|
if (armed.armed && !armed.lockdown) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_ARM_OK;
|
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||||
} else {
|
} 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 {
|
} else {
|
||||||
clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
|
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (vstatus.flag_external_manual_override_ok) {
|
if (vstatus.flag_external_manual_override_ok) {
|
||||||
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
|
||||||
} else {
|
} else {
|
||||||
|
@ -884,11 +901,22 @@ PX4IO::io_get_status()
|
||||||
/* voltage is scaled to mV */
|
/* voltage is scaled to mV */
|
||||||
battery_status.voltage_v = regs[2] / 1000.0f;
|
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 */
|
/* lazily publish the battery voltage */
|
||||||
if (_to_battery > 0) {
|
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_FMU_LOST) ? " FMU_LOST" : ""),
|
||||||
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
|
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
|
||||||
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
|
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
|
||||||
printf("vbatt %u ibatt %u\n",
|
/* now clear alarms */
|
||||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
|
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
|
||||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
|
|
||||||
|
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");
|
printf("actuators");
|
||||||
for (unsigned i = 0; i < _max_actuators; i++)
|
for (unsigned i = 0; i < _max_actuators; i++)
|
||||||
printf(" %u", io_reg_get(PX4IO_PAGE_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);
|
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
|
||||||
printf("arming 0x%04x%s%s%s%s\n",
|
printf("arming 0x%04x%s%s%s%s\n",
|
||||||
arming,
|
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_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" : ""));
|
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
|
||||||
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
|
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_RATES),
|
||||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE),
|
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_PWM_ALTRATE),
|
||||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
|
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("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
|
||||||
printf("controls");
|
printf("controls");
|
||||||
for (unsigned i = 0; i < _max_controls; i++)
|
for (unsigned i = 0; i < _max_controls; i++)
|
||||||
|
@ -1326,21 +1358,27 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
|
||||||
switch (cmd) {
|
switch (cmd) {
|
||||||
case PWM_SERVO_ARM:
|
case PWM_SERVO_ARM:
|
||||||
/* set the 'armed' bit */
|
/* 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;
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_DISARM:
|
case PWM_SERVO_DISARM:
|
||||||
/* clear the 'armed' bit */
|
/* 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;
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SET_UPDATE_RATE:
|
case PWM_SERVO_SET_UPDATE_RATE:
|
||||||
/* set the requested alternate 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);
|
||||||
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
|
|
||||||
} else {
|
|
||||||
ret = -EINVAL;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case PWM_SERVO_SELECT_UPDATE_RATE: {
|
case PWM_SERVO_SELECT_UPDATE_RATE: {
|
||||||
|
@ -1525,6 +1563,12 @@ PX4IO::set_update_rate(int rate)
|
||||||
return 0;
|
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[]);
|
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
@ -1662,6 +1706,18 @@ px4io_main(int argc, char *argv[])
|
||||||
exit(0);
|
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 (!strcmp(argv[1], "recovery")) {
|
||||||
|
|
||||||
if (g_dev != nullptr) {
|
if (g_dev != nullptr) {
|
||||||
|
@ -1789,5 +1845,5 @@ px4io_main(int argc, char *argv[])
|
||||||
monitor();
|
monitor();
|
||||||
|
|
||||||
out:
|
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'");
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,6 +127,8 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
/* this is immediately fatal */
|
/* this is immediately fatal */
|
||||||
log("bootloader not responding");
|
log("bootloader not responding");
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return -EIO;
|
return -EIO;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -145,18 +147,25 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
|
|
||||||
if (filename == NULL) {
|
if (filename == NULL) {
|
||||||
log("no firmware found");
|
log("no firmware found");
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return -ENOENT;
|
return -ENOENT;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct stat st;
|
struct stat st;
|
||||||
if (stat(filename, &st) != 0) {
|
if (stat(filename, &st) != 0) {
|
||||||
log("Failed to stat %s - %d\n", filename, (int)errno);
|
log("Failed to stat %s - %d\n", filename, (int)errno);
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return -errno;
|
return -errno;
|
||||||
}
|
}
|
||||||
fw_size = st.st_size;
|
fw_size = st.st_size;
|
||||||
|
|
||||||
if (_fw_fd == -1)
|
if (_fw_fd == -1) {
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return -ENOENT;
|
return -ENOENT;
|
||||||
|
}
|
||||||
|
|
||||||
/* do the usual program thing - allow for failure */
|
/* do the usual program thing - allow for failure */
|
||||||
for (unsigned retries = 0; retries < 1; retries++) {
|
for (unsigned retries = 0; retries < 1; retries++) {
|
||||||
|
@ -167,6 +176,8 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
if (ret != OK) {
|
if (ret != OK) {
|
||||||
/* this is immediately fatal */
|
/* this is immediately fatal */
|
||||||
log("bootloader not responding");
|
log("bootloader not responding");
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return -EIO;
|
return -EIO;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -178,6 +189,8 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
log("found bootloader revision: %d", bl_rev);
|
log("found bootloader revision: %d", bl_rev);
|
||||||
} else {
|
} else {
|
||||||
log("found unsupported bootloader revision %d, exiting", bl_rev);
|
log("found unsupported bootloader revision %d, exiting", bl_rev);
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -221,6 +234,8 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||||
}
|
}
|
||||||
|
|
||||||
close(_fw_fd);
|
close(_fw_fd);
|
||||||
|
close(_io_fd);
|
||||||
|
_io_fd = -1;
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -32,28 +32,10 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
#
|
#
|
||||||
# ARM CMSIS DSP library
|
# Fixedwing Attitude Control Demo / Example Application
|
||||||
#
|
#
|
||||||
|
|
||||||
#
|
MODULE_COMMAND = ex_fixedwing_control
|
||||||
# Find sources
|
|
||||||
#
|
|
||||||
DSPLIB_SRCDIR := $(PX4_MODULE_SRC)/modules/mathlib/CMSIS
|
|
||||||
ABS_SRCS := $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
|
|
||||||
|
|
||||||
INCLUDE_DIRS += $(DSPLIB_SRCDIR)/Include \
|
SRCS = main.c \
|
||||||
$(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \
|
params.c
|
||||||
$(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
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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);
|
|
@ -37,4 +37,4 @@
|
||||||
|
|
||||||
MODULE_COMMAND = px4_daemon_app
|
MODULE_COMMAND = px4_daemon_app
|
||||||
|
|
||||||
SRCS = px4_daemon_app.c
|
SRCS = px4_daemon_app.c
|
||||||
|
|
|
@ -37,4 +37,4 @@
|
||||||
|
|
||||||
MODULE_COMMAND = px4_mavlink_debug
|
MODULE_COMMAND = px4_mavlink_debug
|
||||||
|
|
||||||
SRCS = px4_mavlink_debug.c
|
SRCS = px4_mavlink_debug.c
|
|
@ -37,4 +37,4 @@
|
||||||
|
|
||||||
MODULE_COMMAND = px4_simple_app
|
MODULE_COMMAND = px4_simple_app
|
||||||
|
|
||||||
SRCS = px4_simple_app.c
|
SRCS = px4_simple_app.c
|
||||||
|
|
|
@ -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>
|
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||||
*
|
*
|
||||||
* Redistribution and use in source and binary forms, with or without
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
@ -47,27 +47,42 @@
|
||||||
*/
|
*/
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
/*
|
/**
|
||||||
* The mavlink log device node; must be opened before messages
|
* The mavlink log device node; must be opened before messages
|
||||||
* can be logged.
|
* can be logged.
|
||||||
*/
|
*/
|
||||||
#define MAVLINK_LOG_DEVICE "/dev/mavlink"
|
#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_INFO _IOC(0x1100, 1)
|
||||||
#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2)
|
#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2)
|
||||||
#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3)
|
#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.
|
* Send a mavlink emergency message.
|
||||||
*
|
*
|
||||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||||
* @param _text The text to log;
|
* @param _text The text to log;
|
||||||
*/
|
*/
|
||||||
#ifdef __cplusplus
|
#define mavlink_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
|
||||||
#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
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Send a mavlink critical message.
|
* Send a mavlink critical message.
|
||||||
|
@ -75,11 +90,7 @@
|
||||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||||
* @param _text The text to log;
|
* @param _text The text to log;
|
||||||
*/
|
*/
|
||||||
#ifdef __cplusplus
|
#define mavlink_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
|
||||||
#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
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Send a mavlink info message.
|
* Send a mavlink info message.
|
||||||
|
@ -87,14 +98,10 @@
|
||||||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||||
* @param _text The text to log;
|
* @param _text The text to log;
|
||||||
*/
|
*/
|
||||||
#ifdef __cplusplus
|
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
|
||||||
#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
|
|
||||||
|
|
||||||
struct mavlink_logmessage {
|
struct mavlink_logmessage {
|
||||||
char text[51];
|
char text[MAVLINK_LOG_MAXLEN + 1];
|
||||||
unsigned char severity;
|
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);
|
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
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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_ */
|
|
@ -94,7 +94,7 @@
|
||||||
#include <drivers/drv_baro.h>
|
#include <drivers/drv_baro.h>
|
||||||
|
|
||||||
#include "calibration_routines.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_LL, 0); /**< Go into low-level failsafe after 0 ms */
|
||||||
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
|
//PARAM_DEFINE_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_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_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_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);
|
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);
|
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);
|
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)
|
void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
{
|
{
|
||||||
/* announce change */
|
/* announce change */
|
||||||
|
@ -797,22 +676,22 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
|
||||||
|
|
||||||
const int calibration_count = 2500;
|
const int calibration_count = 2500;
|
||||||
|
|
||||||
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
|
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||||
struct differential_pressure_s differential_pressure;
|
struct differential_pressure_s diff_pres;
|
||||||
|
|
||||||
int calibration_counter = 0;
|
int calibration_counter = 0;
|
||||||
float airspeed_offset = 0.0f;
|
float diff_pres_offset = 0.0f;
|
||||||
|
|
||||||
while (calibration_counter < calibration_count) {
|
while (calibration_counter < calibration_count) {
|
||||||
|
|
||||||
/* wait blocking for new data */
|
/* 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);
|
int poll_ret = poll(fds, 1, 1000);
|
||||||
|
|
||||||
if (poll_ret) {
|
if (poll_ret) {
|
||||||
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
|
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||||
airspeed_offset += differential_pressure.voltage;
|
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||||
calibration_counter++;
|
calibration_counter++;
|
||||||
|
|
||||||
} else if (poll_ret == 0) {
|
} 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!");
|
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;
|
status->flag_preflight_airspeed_calibration = false;
|
||||||
state_machine_publish(status_pub, status, mavlink_fd);
|
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) {
|
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
|
||||||
mavlink_log_info(mavlink_fd, "CMD starting accel cal");
|
mavlink_log_info(mavlink_fd, "CMD starting accel cal");
|
||||||
tune_confirm();
|
tune_confirm();
|
||||||
do_accel_calibration(status_pub, ¤t_status);
|
do_accel_calibration(status_pub, ¤t_status, mavlink_fd);
|
||||||
tune_confirm();
|
tune_confirm();
|
||||||
mavlink_log_info(mavlink_fd, "CMD finished accel cal");
|
mavlink_log_info(mavlink_fd, "CMD finished accel cal");
|
||||||
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY);
|
||||||
|
@ -1477,10 +1356,10 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
struct sensor_combined_s sensors;
|
struct sensor_combined_s sensors;
|
||||||
memset(&sensors, 0, sizeof(sensors));
|
memset(&sensors, 0, sizeof(sensors));
|
||||||
|
|
||||||
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
|
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||||
struct differential_pressure_s differential_pressure;
|
struct differential_pressure_s diff_pres;
|
||||||
memset(&differential_pressure, 0, sizeof(differential_pressure));
|
memset(&diff_pres, 0, sizeof(diff_pres));
|
||||||
uint64_t last_differential_pressure_time = 0;
|
uint64_t last_diff_pres_time = 0;
|
||||||
|
|
||||||
/* Subscribe to command topic */
|
/* Subscribe to command topic */
|
||||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
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_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) {
|
if (new_data) {
|
||||||
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
|
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||||
last_differential_pressure_time = differential_pressure.timestamp;
|
last_diff_pres_time = diff_pres.timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_check(cmd_sub, &new_data);
|
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 ||
|
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
|
||||||
current_status.state_machine == SYSTEM_STATE_AUTO ||
|
current_status.state_machine == SYSTEM_STATE_AUTO ||
|
||||||
current_status.state_machine == SYSTEM_STATE_MANUAL)) {
|
current_status.state_machine == SYSTEM_STATE_MANUAL)) {
|
||||||
/* armed */
|
/* armed, solid */
|
||||||
led_toggle(LED_BLUE);
|
led_on(LED_AMBER);
|
||||||
|
|
||||||
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||||
/* not armed */
|
/* 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) {
|
if (current_status.flag_hil_enabled) {
|
||||||
/* 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)) {
|
} 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);
|
led_toggle(LED_AMBER);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1754,7 +1651,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Check for valid airspeed/differential pressure measurements */
|
/* 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;
|
current_status.flag_airspeed_valid = true;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -35,7 +35,9 @@
|
||||||
# Main system state machine
|
# Main system state machine
|
||||||
#
|
#
|
||||||
|
|
||||||
MODULE_COMMAND = commander
|
MODULE_COMMAND = commander
|
||||||
SRCS = commander.c \
|
SRCS = commander.c \
|
||||||
state_machine_helper.c \
|
state_machine_helper.c \
|
||||||
calibration_routines.c
|
calibration_routines.c \
|
||||||
|
accelerometer_calibration.c
|
||||||
|
|
||||||
|
|
|
@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
|
||||||
{
|
{
|
||||||
struct actuator_armed_s armed;
|
struct actuator_armed_s armed;
|
||||||
armed.armed = current_status->flag_system_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 */
|
/* lock down actuators if required, only in HIL */
|
||||||
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
|
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
|
||||||
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||||
|
|
|
@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
|
||||||
|
|
||||||
BlockYawDamper::~BlockYawDamper() {};
|
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)));
|
_rWashout.update(_rLowPass.update(r)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
|
||||||
BlockStabilization::~BlockStabilization() {};
|
BlockStabilization::~BlockStabilization() {};
|
||||||
|
|
||||||
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
|
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));
|
pCmd - _pLowPass.update(p));
|
||||||
_elevator = _q2Elv.update(
|
_elevator = outputScale*_q2Elv.update(
|
||||||
qCmd - _qLowPass.update(q));
|
qCmd - _qLowPass.update(q));
|
||||||
_yawDamper.update(rCmd, r);
|
_yawDamper.update(rCmd, r, outputScale);
|
||||||
}
|
}
|
||||||
|
|
||||||
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
|
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
|
||||||
|
@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
|
||||||
_theLimit(this, "THE"),
|
_theLimit(this, "THE"),
|
||||||
_vLimit(this, "V"),
|
_vLimit(this, "V"),
|
||||||
|
|
||||||
// altitude/roc hold
|
// altitude/climb rate hold
|
||||||
_h2Thr(this, "H2THR"),
|
_h2Thr(this, "H2THR"),
|
||||||
_roc2Thr(this, "ROC2THR"),
|
_cr2Thr(this, "CR2THR"),
|
||||||
|
|
||||||
// guidance block
|
// guidance block
|
||||||
_guide(this, ""),
|
_guide(this, ""),
|
||||||
|
|
||||||
// block params
|
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
|
||||||
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
|
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
|
||||||
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
|
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
|
||||||
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
|
_trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
|
||||||
_trimThr(this, "TRIM_THR", true), /* 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"),
|
_vCmd(this, "V_CMD"),
|
||||||
_rocMax(this, "ROC_MAX"),
|
_crMax(this, "CR_MAX"),
|
||||||
_attPoll(),
|
_attPoll(),
|
||||||
_lastPosCmd(),
|
_lastPosCmd(),
|
||||||
_timeStamp(0)
|
_timeStamp(0)
|
||||||
|
@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
||||||
|
|
||||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
// 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
|
// altitude hold
|
||||||
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
|
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
|
||||||
|
@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
|
||||||
// velocity hold
|
// velocity hold
|
||||||
// negative sign because nose over to increase speed
|
// negative sign because nose over to increase speed
|
||||||
float thetaCmd = _theLimit.update(-_v2Theta.update(
|
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
|
||||||
_vLimit.update(_vCmd.get()) - v));
|
|
||||||
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
||||||
|
|
||||||
// yaw rate cmd
|
// yaw rate cmd
|
||||||
float rCmd = 0;
|
float rCmd = 0;
|
||||||
|
|
||||||
// stabilization
|
// 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,
|
_stabilization.update(pCmd, qCmd, rCmd,
|
||||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
|
||||||
|
outputScale);
|
||||||
|
|
||||||
// output
|
// output
|
||||||
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
|
_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) {
|
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||||
|
|
||||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
// 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
|
// pitch channel -> rate of climb
|
||||||
// TODO, might want to put a gain on this, otherwise commanding
|
// TODO, might want to put a gain on this, otherwise commanding
|
||||||
// from +1 -> -1 m/s for rate of climb
|
// from +1 -> -1 m/s for rate of climb
|
||||||
//float dThrottle = _roc2Thr.update(
|
//float dThrottle = _cr2Thr.update(
|
||||||
//_rocMax.get()*_manual.pitch - _pos.vz);
|
//_crMax.get()*_manual.pitch - _pos.vz);
|
||||||
|
|
||||||
// roll channel -> bank angle
|
// roll channel -> bank angle
|
||||||
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
|
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
|
||||||
|
@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
|
||||||
// throttle channel -> velocity
|
// throttle channel -> velocity
|
||||||
// negative sign because nose over to increase speed
|
// negative sign because nose over to increase speed
|
||||||
float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
|
float vCmd = _vLimit.update(_manual.throttle *
|
||||||
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
|
(_vLimit.getMax() - _vLimit.getMin()) +
|
||||||
|
_vLimit.getMin());
|
||||||
|
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
|
||||||
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
||||||
|
|
||||||
// yaw rate cmd
|
// yaw rate cmd
|
||||||
|
|
|
@ -193,7 +193,7 @@ public:
|
||||||
* good idea to declare a member to store the temporary
|
* good idea to declare a member to store the temporary
|
||||||
* variable.
|
* variable.
|
||||||
*/
|
*/
|
||||||
void update(float rCmd, float r);
|
void update(float rCmd, float r, float outputScale = 1.0);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Rudder output value accessor
|
* Rudder output value accessor
|
||||||
|
@ -226,7 +226,8 @@ public:
|
||||||
BlockStabilization(SuperBlock *parent, const char *name);
|
BlockStabilization(SuperBlock *parent, const char *name);
|
||||||
virtual ~BlockStabilization();
|
virtual ~BlockStabilization();
|
||||||
void update(float pCmd, float qCmd, float rCmd,
|
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 getAileron() { return _aileron; }
|
||||||
float getElevator() { return _elevator; }
|
float getElevator() { return _elevator; }
|
||||||
float getRudder() { return _yawDamper.getRudder(); }
|
float getRudder() { return _yawDamper.getRudder(); }
|
||||||
|
@ -310,9 +311,9 @@ private:
|
||||||
BlockLimit _theLimit;
|
BlockLimit _theLimit;
|
||||||
BlockLimit _vLimit;
|
BlockLimit _vLimit;
|
||||||
|
|
||||||
// altitude/ roc hold
|
// altitude/ climb rate hold
|
||||||
BlockPID _h2Thr;
|
BlockPID _h2Thr;
|
||||||
BlockPID _roc2Thr;
|
BlockPID _cr2Thr;
|
||||||
|
|
||||||
// guidance
|
// guidance
|
||||||
BlockWaypointGuidance _guide;
|
BlockWaypointGuidance _guide;
|
||||||
|
@ -322,8 +323,9 @@ private:
|
||||||
BlockParam<float> _trimElv;
|
BlockParam<float> _trimElv;
|
||||||
BlockParam<float> _trimRdr;
|
BlockParam<float> _trimRdr;
|
||||||
BlockParam<float> _trimThr;
|
BlockParam<float> _trimThr;
|
||||||
|
BlockParam<float> _trimV;
|
||||||
BlockParam<float> _vCmd;
|
BlockParam<float> _vCmd;
|
||||||
BlockParam<float> _rocMax;
|
BlockParam<float> _crMax;
|
||||||
|
|
||||||
struct pollfd _attPoll;
|
struct pollfd _attPoll;
|
||||||
vehicle_global_position_setpoint_s _lastPosCmd;
|
vehicle_global_position_setpoint_s _lastPosCmd;
|
||||||
|
|
|
@ -37,4 +37,5 @@
|
||||||
|
|
||||||
MODULE_COMMAND = fixedwing_backside
|
MODULE_COMMAND = fixedwing_backside
|
||||||
|
|
||||||
SRCS = fixedwing_backside_main.cpp
|
SRCS = fixedwing_backside_main.cpp \
|
||||||
|
params.c
|
||||||
|
|
|
@ -59,13 +59,14 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
|
||||||
// rate of climb
|
// rate of climb
|
||||||
// this is what rate of climb is commanded (in m/s)
|
// this is what rate of climb is commanded (in m/s)
|
||||||
// when the pitch stick is fully defelcted in simple mode
|
// 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
|
// climb rate -> thr
|
||||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
|
PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID
|
||||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
|
PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f);
|
||||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
|
PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f);
|
||||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
|
PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f);
|
||||||
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 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
|
||||||
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
|
|
||||||
*/
|
|
|
@ -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
Loading…
Reference in New Issue