Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-23 12:18:12 +08:00
Code Issues Actions 37 Packages Projects Releases Wiki Activity

Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2

Browse Source
This commit is contained in:
px4dev
2013-05-22 21:39:30 +02:00
parent e67022f874 78d29045c4
commit 437d9e4180
364 changed files with 23971 additions and 121563 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+22 -58
View File
@@ -1,62 +1,26 @@
.built
.context
*.context
*.bdat
*.pdat
.depend
.updated
.config
.config-e
.version
.project
.cproject
apps/builtin/builtin_list.h
apps/builtin/builtin_proto.h
Make.dep
*.pyc
*.o
*.a
*.d
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
nuttx/setenv.sh
nuttx/arch/arm/include/board
nuttx/arch/arm/include/chip
nuttx/arch/arm/src/board
nuttx/arch/arm/src/chip
nuttx/include/apps
nuttx/include/arch
nuttx/include/math.h
nuttx/include/nuttx/config.h
nuttx/include/nuttx/version.h
nuttx/tools/mkconfig
nuttx/tools/mkconfig.exe
nuttx/tools/mkversion
nuttx/tools/mkversion.exe
nuttx/nuttx
nuttx/System.map
nuttx/nuttx.bin
nuttx/nuttx.hex
.configured
.settings
Firmware.sublime-workspace
.DS_Store
cscope.out
.configX-e
nuttx-export.zip
.~lock.*
dot.gdbinit
mavlink/include/mavlink/v0.9/
.*.swp
.swp
core
.gdbinit
mkdeps
Archives
Build
!ROMFS/*/*.d
!ROMFS/*/*/*.d
!ROMFS/*/*/*/*.d
*.dSYM
*.o
*.pyc
*~
.*.swp
.context
.cproject
.DS_Store
.gdbinit
.project
.settings
.swp
.~lock.*
Archives/*
Build/*
core
cscope.out
dot.gdbinit
Firmware.sublime-workspace
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/
Documentation/control_flow.graffle
+16650
View File
File diff suppressed because it is too large Load Diff
Makefile
+14 -4
View File
@@ -1,5 +1,5 @@
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -95,9 +95,14 @@ all: $(STAGED_FIRMWARES)
#
# Copy FIRMWARES into the image directory.
#
# XXX copying the .bin files is a hack to work around the PX4IO uploader
# not supporting .px4 files, and it should be deprecated onced that
# is taken care of.
#
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@echo %% Copying $@
$(Q) $(COPY) $< $@
$(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@)
#
# Generate FIRMWARES.
@@ -140,7 +145,7 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),)
endif
$(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@))
$(ARCHIVE_DIR)%.export: configuration = $(if $(findstring px4io,$(board)),io,nsh)
$(ARCHIVE_DIR)%.export: configuration = $(if $(filter px4io px4iov2,$(board)),io,nsh)
$(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
@echo %% Configuring NuttX for $(board)/$(configuration)
$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
@@ -159,11 +164,11 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
.PHONY: clean
clean:
$(Q) $(RMDIR) $(BUILD_DIR)*.build
$(Q) $(REMOVE) -f $(IMAGE_DIR)*.px4
$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
.PHONY: distclean
distclean: clean
$(Q) $(REMOVE) -f $(ARCHIVE_DIR)*.export
$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
#
@@ -196,6 +201,11 @@ help:
@echo " distclean"
@echo " Remove all compilation products, including NuttX RTOS archives."
@echo ""
@echo " upload"
@echo " When exactly one config is being built, add this target to upload the"
@echo " firmware to the board when the build is complete. Not supported for"
@echo " all configurations."
@echo ""
@echo " Common options:"
@echo " ---------------"
@echo ""
ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
+3 -3
View File
@@ -20,10 +20,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#
ROMFS/px4fmu_common/init.d/rc.IO_QUAD
+107
View File
@@ -0,0 +1,107 @@
#!nsh
# Disable USB and autostart
set USB no
set MODE quad
#
# Start the ORB (first app to start)
#
uorb start
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/params
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
#
if [ -f /fs/microsd/px4io.bin ]
then
echo "PX4IO Firmware found. Checking Upgrade.."
if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
then
echo "No newer version, skipping upgrade."
else
echo "Loading /fs/microsd/px4io.bin"
if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log
then
cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current
echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log
else
echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log
echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode"
fi
fi
fi
#
# Start MAVLink (depends on orb)
#
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Start PX4IO interface (depends on orb, commander)
#
px4io start
#
# Allow PX4IO to recover from midair restarts.
# this is very unlikely, but quite safe and robust.
px4io recovery
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude estimator (depends on orb)
#
attitude_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
multirotor_att_control start
#
# Start logging
#
#sdlog start -s 4
#
# Start system state
#
if blinkm start
then
echo "using BlinkM for state indication"
blinkm systemstate
else
echo "no BlinkM found, OK."
fi
ROMFS/px4fmu_common/init.d/rc.PX4IO
+3 -3
View File
@@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#
ROMFS/px4fmu_common/init.d/rc.PX4IOAR
+4 -4
View File
@@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Init the parameter storage
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#
ROMFS/px4fmu_common/init.d/rc.hil
+3 -3
View File
@@ -17,10 +17,10 @@ hil mode_pwm
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#
ROMFS/px4fmu_common/init.d/rc.sensors
+8
View File
@@ -7,6 +7,14 @@
# Start sensor drivers here.
#
#
# Check for UORB
#
if uorb start
then
echo "uORB started"
fi
ms5611 start
adc start
ROMFS/px4fmu_common/mixers/FMU_CCPM.mix
+53
View File
@@ -0,0 +1,53 @@
Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
==================================================
Output 0 - Rear Servo Mixer
----------------
Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
M: 2
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
Output 1 - Left Servo Mixer
-----------------
Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 8660 8660 0 -10000 10000
Output 2 - Right Servo Mixer
----------------
Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 -8660 -8660 0 -10000 10000
Output 3 - Tail Servo Mixer
----------------
Tail Servo = Yaw (control index = 2)
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Output 4 - Motor speed mixer
-----------------
This would be the motor speed control output from governor power demand- not sure what index to use here?
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 0 20000 -10000 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
+6
View File
@@ -0,0 +1,6 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%.
R: 4w 10000 10000 10000 0
apps/.gitignore
+10
View File
@@ -0,0 +1,10 @@
*.a
*.bdat
*.pdat
.built
.config
.depend
.updated
builtin/builtin_list.h
builtin/builtin_proto.h
Make.dep
makefiles/README.txt
+71
View File
@@ -0,0 +1,71 @@
PX4 Build System
================
The files in this directory implement the PX4 runtime firmware build system
and configuration for the standard PX4 boards and software, in conjunction
with Makefile in the parent directory.
../Makefile
Top-level makefile for the PX4 build system. This makefile supports
building NuttX archives, as well as supervising the building of all
of the defined PX4 firmware configurations.
Try 'make help' in the parent directory for documentation.
firmware.mk
Manages the build for one specific firmware configuration.
See the comments at the top of this file for detailed documentation.
Builds modules, builtin command lists and the ROMFS (if configured).
This is the makefile directly used by external build systems; it can
be configured to compile modules both inside and outside the PX4
source tree. When used in this mode, at least BOARD, MODULES and
CONFIG_FILE must be set.
module.mk
Called by firmware.mk to build individual modules.
See the comments at the top of this file for detailed documentation.
Not normally used other than by firmware.mk.
nuttx.mk
Called by ../Makefile to build or download the NuttX archives.
upload.mk
Called by ../Makefile to upload files to a target board. Can be used
by external build systems as well.
setup.mk
Provides common path and tool definitions. Implements host system-specific
compatibility hacks.
board_<boardname>.mk
Board-specific configuration for <boardname>. Typically sets CONFIG_ARCH
and then includes the toolchain definition for the board.
config_<boardname>_<configname>.mk
Parameters for a specific configuration on a specific board.
The board name is derived from the filename. Sets MODULES to select
source modules to be included in the configuration, may also set
ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module
commands (e.g. from NuttX)
toolchain_<toolchainname>.mk
Provides macros used to compile and link source files.
Accepts EXTRADEFINES to add additional pre-processor symbol definitions,
EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass
additional flags to the C compiler, C++ compiler, assembler and linker
respectively.
Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK
macros that are used elsewhere in the build system.
makefiles/config_px4fmu_default.mk
+14 -2
View File
@@ -14,6 +14,7 @@ MODULES += drivers/device
MODULES += drivers/stm32
MODULES += drivers/stm32/adc
MODULES += drivers/stm32/tone_alarm
MODULES += drivers/led
MODULES += drivers/px4io
MODULES += drivers/px4fmu
MODULES += drivers/boards/px4fmu
@@ -29,6 +30,9 @@ MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott_telemetry
MODULES += drivers/blinkm
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/ets_airspeed
MODULES += modules/sensors
#
@@ -77,15 +81,19 @@ MODULES += modules/multirotor_pos_control
MODULES += modules/sdlog
#
# Libraries
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/mathlib
MODULES += modules/mathlib/CMSIS
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += modules/mathlib/CMSIS
#
# Demo apps
#
@@ -102,6 +110,10 @@ MODULES += modules/uORB
# https://pixhawk.ethz.ch/px4/dev/debug_values
#MODULES += examples/px4_mavlink_debug
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
MODULES += examples/fixedwing_control
#
# Transitional support - add commands from the NuttX export archive.
#
makefiles/config_px4fmuv2_default.mk
+6 -2
View File
@@ -74,15 +74,19 @@ MODULES += modules/multirotor_pos_control
MODULES += modules/sdlog
#
# Libraries
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/mathlib
MODULES += modules/mathlib/CMSIS
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += modules/mathlib/CMSIS
#
# Demo apps
#
makefiles/firmware.mk
+83 -28
View File
@@ -180,20 +180,8 @@ EXTRA_CLEANS =
# Modules
################################################################################
#
# We don't actually know what a module is called; all we have is a path fragment
# that we can search for, and where we expect to find a module.mk file.
#
# As such, we replicate the successfully-found path inside WORK_DIR for the
# module's build products in order to keep modules separated from each other.
#
# XXX If this becomes unwieldy or breaks for other reasons, we will need to
# move to allocating directory names and keeping tabs on makefiles via
# the directory name. That will involve arithmetic (it'd probably be time
# for GMSL).
# where to look for modules
MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
# sort and unique the modules list
MODULES := $(sort $(MODULES))
@@ -201,9 +189,9 @@ MODULES := $(sort $(MODULES))
# locate the first instance of a module by full path or by looking on the
# module search path
define MODULE_SEARCH
$(abspath $(firstword $(wildcard $(1)/module.mk) \
$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
MISSING_$1))
$(firstword $(abspath $(wildcard $(1)/module.mk)) \
$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
MISSING_$1)
endef
# make a list of module makefiles and check that we found them all
@@ -223,12 +211,15 @@ MODULE_OBJS := $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module
.PHONY: $(MODULE_OBJS)
$(MODULE_OBJS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(MODULE_OBJS): mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
$(MODULE_OBJS): workdir = $(@D)
$(MODULE_OBJS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
MODULE_WORK_DIR=$(dir $@) \
-C $(workdir) \
MODULE_WORK_DIR=$(workdir) \
MODULE_OBJ=$@ \
MODULE_MK=$(mkfile) \
MODULE_NAME=$(lastword $(subst /, ,$(@D))) \
MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
module
# make a list of phony clean targets for modules
@@ -245,6 +236,66 @@ $(MODULE_CLEANS):
MODULE_MK=$(mkfile) \
clean
################################################################################
# Libraries
################################################################################
# where to look for libraries
LIBRARY_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
# sort and unique the library list
LIBRARIES := $(sort $(LIBRARIES))
# locate the first instance of a library by full path or by looking on the
# library search path
define LIBRARY_SEARCH
$(firstword $(abspath $(wildcard $(1)/library.mk)) \
$(abspath $(foreach search_dir,$(LIBRARY_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/library.mk))) \
MISSING_$1)
endef
# make a list of library makefiles and check that we found them all
LIBRARY_MKFILES := $(foreach library,$(LIBRARIES),$(call LIBRARY_SEARCH,$(library)))
MISSING_LIBRARIES := $(subst MISSING_,,$(filter MISSING_%,$(LIBRARY_MKFILES)))
ifneq ($(MISSING_LIBRARIES),)
$(error Can't find library(s): $(MISSING_LIBRARIES))
endif
# Make a list of the archive files we expect to build from libraries
# Note that this path will typically contain a double-slash at the WORK_DIR boundary; this must be
# preserved as it is used below to get the absolute path for the library.mk file correct.
#
LIBRARY_LIBS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)library.a)
# rules to build module objects
.PHONY: $(LIBRARY_LIBS)
$(LIBRARY_LIBS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_LIBS): mkfile = $(patsubst %library.a,%library.mk,$(relpath))
$(LIBRARY_LIBS): workdir = $(@D)
$(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
-C $(workdir) \
LIBRARY_WORK_DIR=$(workdir) \
LIBRARY_LIB=$@ \
LIBRARY_MK=$(mkfile) \
LIBRARY_NAME=$(lastword $(subst /, ,$(workdir))) \
library
# make a list of phony clean targets for modules
LIBRARY_CLEANS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)/clean)
# rules to clean modules
.PHONY: $(LIBRARY_CLEANS)
$(LIBRARY_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_CLEANS): mkfile = $(patsubst %clean,%library.mk,$(relpath))
$(LIBRARY_CLEANS):
@$(ECHO) %% cleaning using $(mkfile)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
LIBRARY_WORK_DIR=$(dir $@) \
LIBRARY_MK=$(mkfile) \
clean
################################################################################
# NuttX libraries and paths
################################################################################
@@ -266,14 +317,18 @@ endif
#
# Add dependencies on anything in the ROMFS root
ROMFS_DEPS += $(wildcard \
(ROMFS_ROOT)/* \
(ROMFS_ROOT)/*/* \
(ROMFS_ROOT)/*/*/* \
(ROMFS_ROOT)/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/* \
(ROMFS_ROOT)/*/*/*/*/*/*)
ROMFS_IMG = $(WORK_DIR)romfs.img
ROMFS_FILES += $(wildcard \
$(ROMFS_ROOT)/* \
$(ROMFS_ROOT)/*/* \
$(ROMFS_ROOT)/*/*/* \
$(ROMFS_ROOT)/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/* \
$(ROMFS_ROOT)/*/*/*/*/*/*)
ifeq ($(ROMFS_FILES),)
$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
endif
ROMFS_DEPS += $(ROMFS_FILES)
ROMFS_IMG = romfs.img
ROMFS_CSRC = $(ROMFS_IMG:.img=.c)
ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)
@@ -413,8 +468,8 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN)
$(PRODUCT_BIN): $(PRODUCT_ELF)
$(call SYM_TO_BIN,$<,$@)
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS))
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS))
#
# Utility rules
makefiles/library.mk
+169
View File
@@ -0,0 +1,169 @@
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
#
# Framework makefile for PX4 libraries
#
# This makefile is invoked by firmware.mk to build each of the linraries
# that will subsequently be linked into the firmware image.
#
# Applications are built as standard ar archives. Unlike modules,
# all public symbols in library objects are visible across the entire
# firmware stack.
#
# In general, modules should be preferred to libraries when possible.
# Libraries may also be pre-built.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the library's output directory.
#
#
# Variables that can be set by the library's library.mk:
#
#
# SRCS (optional)
#
# Lists the .c, cpp and .S files that should be compiled/assembled to
# produce the library.
#
# PREBUILT_LIB (optional)
#
# Names the prebuilt library in the source directory that should be
# linked into the firmware.
#
# INCLUDE_DIRS (optional, must be appended, ignored if SRCS not set)
#
# The list of directories searched for include files. If non-standard
# includes (e.g. those from another module) are required, paths to search
# can be added here.
#
#
#
# Variables visible to the library's library.mk:
#
# CONFIG
# BOARD
# LIBRARY_WORK_DIR
# LIBRARY_LIB
# LIBRARY_MK
# Anything set in setup.mk, board_$(BOARD).mk and the toolchain file.
# Anything exported from config_$(CONFIG).mk
#
################################################################################
# No user-serviceable parts below.
################################################################################
ifeq ($(LIBRARY_MK),)
$(error No library makefile specified)
endif
$(info %% LIBRARY_MK = $(LIBRARY_MK))
#
# Get the board/toolchain config
#
include $(PX4_MK_DIR)/board_$(BOARD).mk
#
# Get the library's config
#
include $(LIBRARY_MK)
LIBRARY_SRC := $(dir $(LIBRARY_MK))
$(info % LIBRARY_NAME = $(LIBRARY_NAME))
$(info % LIBRARY_SRC = $(LIBRARY_SRC))
$(info % LIBRARY_LIB = $(LIBRARY_LIB))
$(info % LIBRARY_WORK_DIR = $(LIBRARY_WORK_DIR))
#
# Things that, if they change, might affect everything
#
GLOBAL_DEPS += $(MAKEFILE_LIST)
################################################################################
# Build rules
################################################################################
#
# What we're going to build
#
library: $(LIBRARY_LIB)
ifneq ($(PREBUILT_LIB),)
VPATH = $(LIBRARY_SRC)
$(LIBRARY_LIB): $(PREBUILT_LIB) $(GLOBAL_DEPS)
@$(ECHO) "PREBUILT: $(PREBUILT_LIB)"
$(Q) $(COPY) $< $@
else
##
## Object files we will generate from sources
##
OBJS = $(addsuffix .o,$(SRCS))
#
# SRCS -> OBJS rules
#
$(OBJS): $(GLOBAL_DEPS)
vpath %.c $(LIBRARY_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
vpath %.cpp $(LIBRARY_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
vpath %.S $(LIBRARY_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#
# Built product rules
#
$(LIBRARY_LIB): $(OBJS) $(GLOBAL_DEPS)
$(call ARCHIVE,$@,$(OBJS))
endif
#
# Utility rules
#
clean:
$(Q) $(REMOVE) $(LIBRARY_LIB) $(OBJS)
makefiles/module.mk
+15 -24
View File
@@ -35,10 +35,14 @@
# This makefile is invoked by firmware.mk to build each of the modules
# that will subsequently be linked into the firmware image.
#
# Applications are built as prelinked objects with a limited set of exported
# Modules are built as prelinked objects with a limited set of exported
# symbols, as the global namespace is shared between all modules. Normally an
# module will just export one or more <command>_main functions.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the module's output directory.
#
#
# Variables that can be set by the module's module.mk:
@@ -179,26 +183,10 @@ CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
#
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
#
# Locate sources (allows relative source paths in module.mk)
#
define SRC_SEARCH
$(abspath $(firstword $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
endef
ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
ifneq ($(MISSING_SRCS),)
$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
endif
ifeq ($(ABS_SRCS),)
$(error $(MODULE_MK): nothing to compile in SRCS)
endif
#
# Object files we will generate from sources
#
OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
##
## Object files we will generate from sources
##
OBJS = $(addsuffix .o,$(SRCS))
#
# SRCS -> OBJS rules
@@ -206,13 +194,16 @@ OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
$(OBJS): $(GLOBAL_DEPS)
$(filter %.c.o,$(OBJS)): $(MODULE_WORK_DIR)%.c.o: %.c $(GLOBAL_DEPS)
vpath %.c $(MODULE_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
$(filter %.cpp.o,$(OBJS)): $(MODULE_WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS)
vpath %.cpp $(MODULE_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
$(filter %.S.o,$(OBJS)): $(MODULE_WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
vpath %.S $(MODULE_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#
makefiles/toolchain_gnu-arm-eabi.mk
+22 -2
View File
@@ -144,6 +144,7 @@ CFLAGS = $(ARCHCFLAGS) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
$(EXTRADEFINES) \
$(EXTRACFLAGS) \
-fno-common \
$(addprefix -I,$(INCLUDE_DIRS))
@@ -156,18 +157,22 @@ CXXFLAGS = $(ARCHCXXFLAGS) \
$(ARCHXXINCLUDES) \
$(INSTRUMENTATIONDEFINES) \
$(ARCHDEFINES) \
$(EXTRADEFINES) \
-DCONFIG_WCHAR_BUILTIN \
$(EXTRADEFINES) \
$(EXTRACXXFLAGS) \
$(addprefix -I,$(INCLUDE_DIRS))
# Flags we pass to the assembler
#
AFLAGS = $(CFLAGS) -D__ASSEMBLY__
AFLAGS = $(CFLAGS) -D__ASSEMBLY__ \
$(EXTRADEFINES) \
$(EXTRAAFLAGS)
# Flags we pass to the linker
#
LDFLAGS += --warn-common \
--gc-sections \
$(EXTRALDFLAGS) \
$(addprefix -T,$(LDSCRIPT)) \
$(addprefix -L,$(LIB_DIRS))
@@ -249,6 +254,20 @@ endef
# - relink the object and insert the binary file
# - edit symbol names to suit
#
# NOTE: exercise caution using this with absolute pathnames; it looks
# like the MinGW tools insert an extra _ in the binary symbol name; e.g.
# the path:
#
# /d/px4/firmware/Build/px4fmu_default.build/romfs.img
#
# is assigned symbols like:
#
# _binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
# when we would expect
#
# _binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
#
define BIN_SYM_PREFIX
_binary_$(subst /,_,$(subst .,_,$1))
endef
@@ -262,4 +281,5 @@ define BIN_TO_OBJ
--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
$(Q) $(REMOVE) $2.c $2.c.o
endef
makefiles/upload.mk
-3
View File
@@ -30,9 +30,6 @@ upload-serial-px4fmu: $(BUNDLE) $(UPLOADER)
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
$(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
upload-serial-px4fmuv2: $(BUNDLE) $(UPLOADER)
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE)
#
# JTAG firmware uploading with OpenOCD
#
nuttx/.gitignore
+28
View File
@@ -0,0 +1,28 @@
*.a
.config
.config-e
.configX-e
.depend
.version
arch/arm/include/board
arch/arm/include/chip
arch/arm/src/board
arch/arm/src/chip
include/apps
include/arch
include/math.h
include/nuttx/config.h
include/nuttx/version.h
Make.defs
Make.dep
mkdeps
nuttx
nuttx-export.zip
nuttx.bin
nuttx.hex
setenv.sh
System.map
tools/mkconfig
tools/mkconfig.exe
tools/mkversion
tools/mkversion.exe
nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
-1
View File
@@ -1512,7 +1512,6 @@ static inline void stm32_ep0out_receive(FAR struct stm32_ep_s *privep, int bcnt)
DEBUGASSERT(privep && privep->ep.priv);
priv = (FAR struct stm32_usbdev_s *)privep->ep.priv;
DEBUGASSERT(priv->ep0state == EP0STATE_SETUP_OUT);
ullvdbg("EP0: bcnt=%d\n", bcnt);
usbtrace(TRACE_READ(EP0), bcnt);
nuttx/configs/px4fmu/nsh/defconfig
+4
View File
@@ -647,10 +647,14 @@ CONFIG_DISABLE_POLL=n
# CONFIG_LIBC_FIXEDPRECISION - Sets 7 digits after dot for printing:
# 5.1234567
# CONFIG_HAVE_LONG_LONG - Enabled printf("%llu)
# CONFIG_LIBC_STRERR - allow printing of error text
# CONFIG_LIBC_STRERR_SHORT - allow printing of short error text
#
CONFIG_NOPRINTF_FIELDWIDTH=n
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_HAVE_LONG_LONG=y
CONFIG_LIBC_STRERROR=n
CONFIG_LIBC_STRERROR_SHORT=n
#
# Allow for architecture optimized implementations
nuttx/libc/stdio/lib_libdtoa.c
+29 -3
View File
@@ -43,6 +43,7 @@
/****************************************************************************
* Included Files
****************************************************************************/
#include <math.h>
/****************************************************************************
* Pre-processor Definitions
@@ -104,6 +105,13 @@ static void zeroes(FAR struct lib_outstream_s *obj, int nzeroes)
* Private Functions
****************************************************************************/
static void lib_dtoa_string(FAR struct lib_outstream_s *obj, const char *str)
{
while (*str) {
obj->put(obj, *str++);
}
}
/****************************************************************************
* Name: lib_dtoa
*
@@ -137,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 dsgn; /* Unused sign indicator */
int i;
bool done_decimal_point = false;
/* special handling for NaN and Infinity */
if (isnan(value)) {
lib_dtoa_string(obj, "NaN");
return;
}
if (isinf(value)) {
if (value < 0.0d) {
obj->put(obj, '-');
}
lib_dtoa_string(obj, "Infinity");
return;
}
/* Non-zero... positive or negative */
if (value < 0)
{
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))
{
obj->put(obj, '.');
done_decimal_point = true;
/* Always print at least one digit to the right of the decimal point. */
@@ -203,6 +226,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
done_decimal_point = true;
/* Print any leading zeros to the right of the decimal point */
@@ -249,6 +273,7 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
/* Print the decimal point */
obj->put(obj, '.');
done_decimal_point = true;
/* Always print at least one digit to the right of the decimal
* point.
@@ -285,8 +310,9 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
}
/* Finally, print any trailing zeroes */
zeroes(obj, prec);
if (done_decimal_point) {
zeroes(obj, prec);
}
/* Is this memory supposed to be freed or not? */
nuttx/libc/stdio/lib_libvsprintf.c
+6 -1
View File
@@ -1215,7 +1215,7 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
fmt = FMT_RJUST;
width = 0;
#ifdef CONFIG_LIBC_FLOATINGPOINT
trunc = 0;
trunc = 6;
#endif
#endif
@@ -1245,6 +1245,11 @@ int lib_vsprintf(FAR struct lib_outstream_s *obj, FAR const char *src, va_list a
{
#ifndef CONFIG_NOPRINTF_FIELDWIDTH
fmt = FMT_RJUST0;
#ifdef CONFIG_LIBC_FLOATINGPOINT
if (IS_HASDOT(flags)) {
trunc = 0;
}
#endif
#endif
}
#if 0
src/drivers/ardrone_interface/ardrone_motor_control.c
+4 -4
View File
@@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
/* send motors via UART */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
src/drivers/boards/px4fmu/module.mk
+2 -1
View File
@@ -6,4 +6,5 @@ SRCS = px4fmu_can.c \
px4fmu_init.c \
px4fmu_pwm_servo.c \
px4fmu_spi.c \
px4fmu_usb.c
px4fmu_usb.c \
px4fmu_led.c
src/drivers/boards/px4fmu/px4fmu_init.c
+20 -7
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -91,6 +91,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
@@ -114,7 +127,7 @@ __EXPORT void stm32_boardinitialize(void)
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure LEDs */
/* configure LEDs (empty call to NuttX' ledinit) */
up_ledinit();
}
@@ -178,11 +191,11 @@ __EXPORT int nsh_archinitialize(void)
(hrt_callout)stm32_serial_dma_poll,
NULL);
// initial LED state
// drv_led_start();
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_on(LED_BLUE);
/* Configure SPI-based devices */
src/drivers/boards/px4fmu/px4fmu_led.c
+18 -10
View File
@@ -39,19 +39,27 @@
#include <nuttx/config.h>
#include <stdint.h>
#include <stdbool.h>
#include <debug.h>
#include <arch/board/board.h>
#include "chip.h"
#include "up_arch.h"
#include "up_internal.h"
#include "stm32_internal.h"
#include "px4fmu_internal.h"
__EXPORT void up_ledinit()
#include <arch/board/board.h>
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
__EXPORT void led_init()
{
/* Configure LED1-2 GPIOs for output */
@@ -59,7 +67,7 @@ __EXPORT void up_ledinit()
stm32_configgpio(GPIO_LED2);
}
__EXPORT void up_ledon(int led)
__EXPORT void led_on(int led)
{
if (led == 0)
{
@@ -73,7 +81,7 @@ __EXPORT void up_ledon(int led)
}
}
__EXPORT void up_ledoff(int led)
__EXPORT void led_off(int led)
{
if (led == 0)
{
src/drivers/device/i2c.h
+9
View File
@@ -53,6 +53,15 @@ namespace device __EXPORT
class __EXPORT I2C : public CDev
{
public:
/**
* Get the address
*/
uint16_t get_address() {
return _address;
}
protected:
/**
* The number of times a read or write operation will be retried on
src/drivers/drv_airspeed.h
+61
View File
@@ -0,0 +1,61 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Airspeed driver interface.
* @author Simon Wilks
*/
#ifndef _DRV_AIRSPEED_H
#define _DRV_AIRSPEED_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define AIRSPEED_DEVICE_PATH "/dev/airspeed"
/*
* ioctl() definitions
*
* Airspeed drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _AIRSPEEDIOCBASE (0x7700)
#define __AIRSPEEDIOC(_n) (_IOC(_AIRSPEEDIOCBASE, _n))
#endif /* _DRV_AIRSPEED_H */
src/drivers/drv_pwm_output.h
+6
View File
@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/ets_airspeed/ets_airspeed.cpp
+832
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_ekf/Makefile → src/drivers/ets_airspeed/module.mk
Executable → Regular
+7 -23
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -31,27 +31,11 @@
#
############################################################################
APPNAME = attitude_estimator_ekf
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
#
# Makefile to build the Eagle Tree Airspeed V3 driver.
#
CXXSRCS = attitude_estimator_ekf_main.cpp
MODULE_COMMAND = ets_airspeed
MODULE_STACKSIZE = 1024
CSRCS = attitude_estimator_ekf_params.c \
codegen/eye.c \
codegen/attitudeKalmanfilter.c \
codegen/mrdivide.c \
codegen/rdivide.c \
codegen/attitudeKalmanfilter_initialize.c \
codegen/attitudeKalmanfilter_terminate.c \
codegen/rt_nonfinite.c \
codegen/rtGetInf.c \
codegen/rtGetNaN.c \
codegen/norm.c \
codegen/cross.c
# XXX this is *horribly* broken
INCLUDES += $(TOPDIR)/../mavlink/include/mavlink
include $(APPDIR)/mk/app.mk
SRCS = ets_airspeed.cpp
src/drivers/gps/gps.cpp
+10 -1
View File
@@ -285,6 +285,10 @@ GPS::task_main()
unlock();
if (_Helper->configure(_baudrate) == 0) {
unlock();
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
@@ -301,6 +305,8 @@ GPS::task_main()
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_Helper->store_update_rates();
_Helper->reset_update_rates();
}
if (!_healthy) {
@@ -372,7 +378,10 @@ GPS::print_info()
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
}
usleep(100000);
src/drivers/gps/gps_helper.cpp
+32 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -36,9 +36,39 @@
#include <termios.h>
#include <errno.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "gps_helper.h"
/* @file gps_helper.cpp */
/**
* @file gps_helper.cpp
*/
float
GPS_Helper::get_position_update_rate()
{
return _rate_lat_lon;
}
float
GPS_Helper::get_velocity_update_rate()
{
return _rate_vel;
}
float
GPS_Helper::reset_update_rates()
{
_rate_count_vel = 0;
_rate_count_lat_lon = 0;
_interval_rate_start = hrt_absolute_time();
}
float
GPS_Helper::store_update_rates()
{
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
}
int
GPS_Helper::set_baudrate(const int &fd, unsigned baud)
src/drivers/gps/gps_helper.h
+19 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -33,7 +33,9 @@
*
****************************************************************************/
/* @file gps_helper.h */
/**
* @file gps_helper.h
*/
#ifndef GPS_HELPER_H
#define GPS_HELPER_H
@@ -44,9 +46,22 @@
class GPS_Helper
{
public:
virtual int configure(unsigned &baud) = 0;
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
int set_baudrate(const int &fd, unsigned baud);
float get_position_update_rate();
float get_velocity_update_rate();
float reset_update_rates();
float store_update_rates();
protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
float _rate_lat_lon;
float _rate_vel;
uint64_t _interval_rate_start;
};
#endif /* GPS_HELPER_H */
src/drivers/gps/mtk.cpp
+4
View File
@@ -263,6 +263,10 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time
_rate_count_vel++;
_rate_count_lat_lon++;
return;
}
src/drivers/gps/mtk.h
+3 -3
View File
@@ -87,14 +87,14 @@ class MTK : public GPS_Helper
public:
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
int parse_char(uint8_t b, gps_mtk_packet_t &packet);
/**
* Handle the package once it has arrived
src/drivers/gps/ubx.cpp
+97 -67
View File
@@ -60,7 +60,8 @@
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_waiting_for_ack(false)
_waiting_for_ack(false),
_disable_cmd_counter(0)
{
decode_init();
}
@@ -139,12 +140,12 @@ UBX::configure(unsigned &baudrate)
cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE;
cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
@@ -164,74 +165,41 @@ UBX::configure(unsigned &baudrate)
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.clsID = UBX_CLASS_CFG;
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
/* For satelites info 1Hz is enough */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
/* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
1);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
// 0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
0);
// /* insist of receiving the ACK for this packet */
// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
// continue;
_waiting_for_ack = false;
return 0;
@@ -239,6 +207,15 @@ UBX::configure(unsigned &baudrate)
return -1;
}
int
UBX::wait_for_ack(unsigned timeout)
{
_waiting_for_ack = true;
int ret = receive(timeout);
_waiting_for_ack = false;
return ret;
}
int
UBX::receive(unsigned timeout)
{
@@ -498,6 +475,8 @@ UBX::handle_message()
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_rate_count_lat_lon++;
/* Add timestamp to finish the report */
_gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */
@@ -653,6 +632,8 @@ UBX::handle_message()
_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
_gps_position->vel_ned_valid = true;
_gps_position->timestamp_velocity = hrt_absolute_time();
_rate_count_vel++;
}
break;
@@ -693,6 +674,12 @@ UBX::handle_message()
default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
if (_disable_cmd_counter++ == 0) {
// Don't attempt for every message to disable, some might not be disabled */
warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0);
}
return ret;
ret = -1;
break;
}
@@ -736,6 +723,25 @@ UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
message[length-1] = ck_b;
}
void
UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
{
for (unsigned i = 0; i < length; i++) {
ck_a = ck_a + message[i];
ck_b = ck_b + ck_a;
}
}
void
UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{
struct ubx_cfg_msg_rate msg;
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
@@ -753,3 +759,27 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
warnx("ubx: config write fail");
}
void
UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
{
struct ubx_header header;
uint8_t ck_a=0, ck_b=0;
header.sync1 = UBX_SYNC1;
header.sync2 = UBX_SYNC2;
header.msg_class = msg_class;
header.msg_id = msg_id;
header.length = size;
add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
// Configure receive check
_clsID_needed = msg_class;
_msgID_needed = msg_id;
write(_fd, (const char *)&header, sizeof(header));
write(_fd, (const char *)msg, size);
write(_fd, (const char *)&ck_a, 1);
write(_fd, (const char *)&ck_b, 1);
}
src/drivers/gps/ubx.h
+51 -27
View File
@@ -65,26 +65,27 @@
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
#define UBX_CFG_RATE_LENGTH 6
#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
@@ -92,6 +93,14 @@
/** the structures of the binary packets */
#pragma pack(push, 1)
struct ubx_header {
uint8_t sync1;
uint8_t sync2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */
@@ -274,11 +283,17 @@ typedef struct {
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t rate[6];
uint8_t rate;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t;
struct ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
// END the structures of the binary packets
// ************
@@ -341,55 +356,64 @@ class UBX : public GPS_Helper
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b);
int parse_char(uint8_t b);
/**
* Handle the package once it has arrived
*/
int handle_message(void);
int handle_message(void);
/**
* Reset the parse state machine for a fresh start
*/
void decode_init(void);
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
void add_byte_to_checksum(uint8_t);
/**
* Add the two checksum bytes to an outgoing message
*/
void add_checksum_to_message(uint8_t* message, const unsigned length);
void add_checksum_to_message(uint8_t* message, const unsigned length);
/**
* Helper to send a config packet
*/
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
int _fd;
void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
ubx_message_class_t _message_class;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
ubx_message_class_t _message_class;
ubx_message_id_t _message_id;
unsigned _payload_size;
unsigned _payload_size;
uint8_t _disable_cmd_counter;
};
#endif /* UBX_H_ */
src/drivers/hmc5883/hmc5883.cpp
+10 -4
View File
@@ -1225,19 +1225,25 @@ start()
/* create the driver, attempt expansion bus first */
g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
if (g_dev != nullptr && OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (g_dev == nullptr)
if (g_dev == nullptr) {
g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
if (g_dev != nullptr && OK != g_dev->init()) {
goto fail;
}
}
#endif
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
src/drivers/hott_telemetry/messages.c
+12
View File
@@ -42,9 +42,11 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
static int airspeed_sub = -1;
static int battery_sub = -1;
static int sensor_sub = -1;
@@ -52,6 +54,7 @@ void messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void build_eam_response(uint8_t *buffer, int *size)
@@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
src/drivers/led/led.cpp
+13 -8
View File
@@ -41,12 +41,17 @@
#include <drivers/device/device.h>
#include <drivers/drv_led.h>
/* Ideally we'd be able to get these from up_internal.h */
//#include <up_internal.h>
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void up_ledinit();
extern void up_ledon(int led);
extern void up_ledoff(int led);
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
class LED : device::CDev
@@ -74,7 +79,7 @@ int
LED::init()
{
CDev::init();
up_ledinit();
led_init();
return 0;
}
@@ -86,11 +91,11 @@ LED::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case LED_ON:
up_ledon(arg);
led_on(arg);
break;
case LED_OFF:
up_ledoff(arg);
led_off(arg);
break;
default:
src/drivers/md25/md25.cpp
+553
View File
File diff suppressed because it is too large Load Diff
src/drivers/md25/md25.hpp
+293
View File
@@ -0,0 +1,293 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file md25.cpp
*
* Driver for MD25 I2C Motor Driver
*
* references:
* http://www.robot-electronics.co.uk/htm/md25tech.htm
* http://www.robot-electronics.co.uk/files/rpi_md25.c
*
*/
#pragma once
#include <poll.h>
#include <stdio.h>
#include <controllib/block/UOrbSubscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <drivers/device/i2c.h>
/**
* This is a driver for the MD25 motor controller utilizing the I2C interface.
*/
class MD25 : public device::I2C
{
public:
/**
* modes
*
* NOTE: this driver assumes we are always
* in mode 0!
*
* seprate speed mode:
* motor speed1 = speed1
* motor speed2 = speed2
* turn speed mode:
* motor speed1 = speed1 + speed2
* motor speed2 = speed2 - speed2
* unsigned:
* full rev (0), stop(128), full fwd (255)
* signed:
* full rev (-127), stop(0), full fwd (128)
*
* modes numbers:
* 0 : unsigned separate (default)
* 1 : signed separate
* 2 : unsigned turn
* 3 : signed turn
*/
enum e_mode {
MODE_UNSIGNED_SPEED = 0,
MODE_SIGNED_SPEED,
MODE_UNSIGNED_SPEED_TURN,
MODE_SIGNED_SPEED_TURN,
};
/** commands */
enum e_cmd {
CMD_RESET_ENCODERS = 32,
CMD_DISABLE_SPEED_REGULATION = 48,
CMD_ENABLE_SPEED_REGULATION = 49,
CMD_DISABLE_TIMEOUT = 50,
CMD_ENABLE_TIMEOUT = 51,
CMD_CHANGE_I2C_SEQ_0 = 160,
CMD_CHANGE_I2C_SEQ_1 = 170,
CMD_CHANGE_I2C_SEQ_2 = 165,
};
/** control channels */
enum e_channels {
CH_SPEED_LEFT = 0,
CH_SPEED_RIGHT
};
/**
* constructor
* @param deviceName the name of the device e.g. "/dev/md25"
* @param bus the I2C bus
* @param address the adddress on the I2C bus
* @param speed the speed of the I2C communication
*/
MD25(const char *deviceName,
int bus,
uint16_t address,
uint32_t speed = 100000);
/**
* deconstructor
*/
virtual ~MD25();
/**
* @return software version
*/
uint8_t getVersion();
/**
* @return speed of motor, normalized (-1, 1)
*/
float getMotor1Speed();
/**
* @return speed of motor 2, normalized (-1, 1)
*/
float getMotor2Speed();
/**
* @return number of rotations since reset
*/
float getRevolutions1();
/**
* @return number of rotations since reset
*/
float getRevolutions2();
/**
* @return battery voltage, volts
*/
float getBatteryVolts();
/**
* @return motor 1 current, amps
*/
float getMotor1Current();
/**
* @return motor 2 current, amps
*/
float getMotor2Current();
/**
* @return the motor acceleration
* controls motor speed change (1-10)
* accel rate | time for full fwd. to full rev.
* 1 | 6.375 s
* 2 | 1.6 s
* 3 | 0.675 s
* 5(default) | 1.275 s
* 10 | 0.65 s
*/
uint8_t getMotorAccel();
/**
* @return motor output mode
* */
e_mode getMode();
/**
* @return current command register value
*/
e_cmd getCommand();
/**
* resets the encoders
* @return non-zero -> error
* */
int resetEncoders();
/**
* enable/disable speed regulation
* @return non-zero -> error
*/
int setSpeedRegulation(bool enable);
/**
* set the timeout for the motors
* enable/disable timeout (motor stop)
* after 2 sec of no i2c messages
* @return non-zero -> error
*/
int setTimeout(bool enable);
/**
* sets the device address
* can only be done with one MD25
* on the bus
* @return non-zero -> error
*/
int setDeviceAddress(uint8_t address);
/**
* set motor 1 speed
* @param normSpeed normalize speed between -1 and 1
* @return non-zero -> error
*/
int setMotor1Speed(float normSpeed);
/**
* set motor 2 speed
* @param normSpeed normalize speed between -1 and 1
* @return non-zero -> error
*/
int setMotor2Speed(float normSpeed);
/**
* main update loop that updates MD25 motor
* speeds based on actuator publication
*/
void update();
/**
* probe for device
*/
virtual int probe();
/**
* search for device
*/
int search();
/**
* read data from i2c
*/
int readData();
/**
* print status
*/
void status(char *string, size_t n);
private:
/** poll structure for control packets */
struct pollfd _controlPoll;
/** actuator controls subscription */
control::UOrbSubscription<actuator_controls_s> _actuators;
// local copy of data from i2c device
uint8_t _version;
float _motor1Speed;
float _motor2Speed;
float _revolutions1;
float _revolutions2;
float _batteryVoltage;
float _motor1Current;
float _motor2Current;
uint8_t _motorAccel;
e_mode _mode;
e_cmd _command;
// private methods
int _writeUint8(uint8_t reg, uint8_t value);
int _writeInt8(uint8_t reg, int8_t value);
float _uint8ToNorm(uint8_t value);
uint8_t _normToUint8(float value);
/**
* set motor control mode,
* this driver assumed we are always in mode 0
* so we don't let the user change the mode
* @return non-zero -> error
*/
int _setMode(e_mode);
};
// unit testing
int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/md25/md25_main.cpp
+264
View File
@@ -0,0 +1,264 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @ file md25.cpp
*
* Driver for MD25 I2C Motor Driver
*
* references:
* http://www.robot-electronics.co.uk/htm/md25tech.htm
* http://www.robot-electronics.co.uk/files/rpi_md25.c
*
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <arch/board/board.h>
#include "md25.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int md25_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int md25_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int md25_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("md25 already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("md25",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
md25_thread_main,
(const char **)argv);
exit(0);
}
if (!strcmp(argv[1], "test")) {
if (argc < 4) {
printf("usage: md25 test bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
md25Test(deviceName, bus, address);
exit(0);
}
if (!strcmp(argv[1], "probe")) {
if (argc < 4) {
printf("usage: md25 probe bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
int ret = md25.probe();
if (ret == OK) {
printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address());
} else {
printf("MD25 not found on bus %d\n", bus);
}
exit(0);
}
if (!strcmp(argv[1], "search")) {
if (argc < 3) {
printf("usage: md25 search bus\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t address = strtoul(argv[3], nullptr, 0);
MD25 md25(deviceName, bus, address);
md25.search();
exit(0);
}
if (!strcmp(argv[1], "change_address")) {
if (argc < 5) {
printf("usage: md25 change_address bus old_address new_address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[2], nullptr, 0);
uint8_t old_address = strtoul(argv[3], nullptr, 0);
uint8_t new_address = strtoul(argv[4], nullptr, 0);
MD25 md25(deviceName, bus, old_address);
int ret = md25.setDeviceAddress(new_address);
if (ret == OK) {
printf("MD25 new address set to 0x%X\n", new_address);
} else {
printf("MD25 failed to set address to 0x%X\n", new_address);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tmd25 app is running\n");
} else {
printf("\tmd25 app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int md25_thread_main(int argc, char *argv[])
{
printf("[MD25] starting\n");
if (argc < 5) {
// extra md25 in arg list since this is a thread
printf("usage: md25 start bus address\n");
exit(0);
}
const char *deviceName = "/dev/md25";
uint8_t bus = strtoul(argv[3], nullptr, 0);
uint8_t address = strtoul(argv[4], nullptr, 0);
// start
MD25 md25("/dev/md25", bus, address);
thread_running = true;
// loop
while (!thread_should_exit) {
md25.update();
}
// exit
printf("[MD25] exiting.\n");
thread_running = false;
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/md25/module.mk
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# MD25 I2C Based Motor Controller
# http://www.robot-electronics.co.uk/htm/md25tech.htm
#
MODULE_COMMAND = md25
SRCS = md25.cpp \
md25_main.cpp
src/drivers/mkblctrl/mkblctrl.cpp
+1442
View File
File diff suppressed because it is too large Load Diff
src/drivers/mkblctrl/module.mk
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012,2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Interface driver for the Mikrokopter BLCtrl
#
MODULE_COMMAND = mkblctrl
SRCS = mkblctrl.cpp
INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common

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