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

Merged

This commit is contained in:
Lorenz Meier 2013-01-20 10:58:30 +01:00
parent 5ba54cd921 48e497e406
commit 3a80ac5938
1071 changed files with 183644 additions and 8236 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.gitignore vendored
View File

@ -8,9 +8,12 @@
apps/namedapp/namedapp_list.h
apps/namedapp/namedapp_proto.h
Make.dep
*.pyc
*.o
*.a
*.d
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
@ -41,3 +44,10 @@ cscope.out
.configX-e
nuttx-export.zip
.~lock.*
dot.gdbinit
mavlink/include/mavlink/v0.9/
.*.swp
.swp
core
.gdbinit
mkdeps

54
Debug/PX4 Normal file
View File

@ -0,0 +1,54 @@
#
# Various PX4-specific macros
#
source Debug/NuttX
echo Loading PX4 GDB macros. Use 'help px4' for more information.\n
define px4
echo Use 'help px4' for more information.\n
end
document px4
. Various macros for working with the PX4 firmware.
.
. perf
. Prints the state of all performance counters.
.
. Use 'help <macro>' for more specific help.
end
define _perf_print
set $hdr = (struct perf_ctr_header *)$arg0
printf "%p\n", $hdr
printf "%s: ", $hdr->name
# PC_COUNT
if $hdr->type == 0
set $count = (struct perf_ctr_count *)$hdr
printf "%llu events,\n", $count->event_count;
end
# PC_ELPASED
if $hdr->type == 1
set $elapsed = (struct perf_ctr_elapsed *)$hdr
printf "%llu events, %lluus elapsed, min %lluus, max %lluus\n", $elapsed->event_count, $elapsed->time_total, $elapsed->time_least, $elapsed->time_most
end
# PC_INTERVAL
if $hdr->type == 2
set $interval = (struct perf_ctr_interval *)$hdr
printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, ($interval->time_last - $interval->time_first) / $interval->event_count, $interval->time_least, $interval->time_most
end
end
define perf
set $ctr = (sq_entry_t *)(perf_counters.head)
while $ctr != 0
_perf_print $ctr
set $ctr = $ctr->flink
end
end
document perf
. perf
. Prints performance counters.
end

31
Makefile
View File

@ -28,12 +28,8 @@ UPLOADER = $(PX4BASE)/Tools/px_uploader.py
# What are we currently configured for?
#
CONFIGURED = $(PX4BASE)/.configured
ifeq ($(wildcard $(CONFIGURED)),)
# the $(CONFIGURED) target will make this a reality before building
export TARGET = px4fmu
$(shell echo $(TARGET) > $(CONFIGURED))
else
export TARGET = $(shell cat $(CONFIGURED))
ifneq ($(wildcard $(CONFIGURED)),)
export TARGET := $(shell cat $(CONFIGURED))
endif
#
@ -59,12 +55,13 @@ $(FIRMWARE_BUNDLE): $(FIRMWARE_BINARY) $(MKFW) $(FIRMWARE_PROTOTYPE)
@$(MKFW) --prototype $(FIRMWARE_PROTOTYPE) \
--git_identity $(PX4BASE) \
--image $(FIRMWARE_BINARY) > $@
#
# Build the firmware binary.
#
.PHONY: $(FIRMWARE_BINARY)
$(FIRMWARE_BINARY): configure_$(TARGET) setup_$(TARGET)
@echo Building $@
$(FIRMWARE_BINARY): setup_$(TARGET) configure-check
@echo Building $@ for $(TARGET)
@make -C $(NUTTX_SRC) -r $(MQUIET) all
@cp $(NUTTX_SRC)/nuttx.bin $@
@ -73,19 +70,26 @@ $(FIRMWARE_BINARY): configure_$(TARGET) setup_$(TARGET)
# and makes it current.
#
configure_px4fmu:
ifneq ($(TARGET),px4fmu)
@echo Configuring for px4fmu
@make -C $(PX4BASE) distclean
endif
@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu/nsh
@echo px4fmu > $(CONFIGURED)
configure_px4io:
ifneq ($(TARGET),px4io)
@echo Configuring for px4io
@make -C $(PX4BASE) distclean
endif
@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io
@echo px4io > $(CONFIGURED)
configure-check:
ifeq ($(wildcard $(CONFIGURED)),)
@echo
@echo "Not configured - use 'make configure_px4fmu' or 'make configure_px4io' first"
@echo
@exit 1
endif
#
# Per-configuration additional targets
#
@ -96,6 +100,9 @@ setup_px4fmu:
setup_px4io:
# fake target to make configure-check happy if TARGET is not set
setup_:
#
# Firmware uploading.
#

54
ROMFS/logging/logconv.m
View File

@ -9,6 +9,13 @@ close all
% Set the path to your sysvector.bin file here
filePath = 'sysvector.bin';
% Work around a Matlab bug (not related to PX4)
% where timestamps from 1.1.1970 do not allow to
% read the file's size
if ismac
system('touch -t 201212121212.12 sysvector.bin');
end
%%%%%%%%%%%%%%%%%%%%%%%
% SYSTEM VECTOR
%
@ -24,6 +31,8 @@ filePath = 'sysvector.bin';
% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
% float vbat; //battery voltage in [volt]
% float bat_current - current drawn from battery at this time instant
% float bat_discharged - discharged energy in mAh
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
@ -31,27 +40,34 @@ filePath = 'sysvector.bin';
% float rotMatrix[9]; //unitvectors
% float actuator_control[4]; //unitvector
% float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
% float diff_pressure; - pressure difference in millibar
% float ind_airspeed;
% float true_airspeed;
% Definition of the logged values
logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{17} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{18} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
% First get length of one line
columns = length(logFormat);
@ -96,5 +112,3 @@ if exist(filePath, 'file')
else
disp(['file: ' filePath ' does not exist' char(10)]);
end

46
ROMFS/scripts/rc.PX4IO
View File

@ -1,12 +1,9 @@
#!nsh
#
# Flight startup script for PX4FMU with PX4IO carrier board.
#
echo "[init] doing PX4IO startup..."
set USB no
#
# Start the ORB
# Start the object request broker
#
uorb start
@ -20,15 +17,27 @@ then
param load
fi
#
# Enable / connect to PX4IO
#
px4io start
#
# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
# which is good for testing. See ROMFS/mixers for a full list of mixers.
#
mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink
# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the commander.
@ -41,35 +50,24 @@ commander start
attitude_estimator_ekf start
#
# Configure PX4FMU for operation with PX4IO
# Start the attitude and position controller
#
# XXX arguments?
#
px4fmu start
fixedwing_att_control start
fixedwing_pos_control start
#
# Start the fixed-wing controller
#
fixedwing_control start
#
# Fire up the PX4IO interface.
#
px4io start
#
# Start looking for a GPS.
# Start GPS capture. Comment this out if you do not have a GPS.
#
gps start
#
# Start logging to microSD if we can
#
#
sh /etc/init.d/rc.logging
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry (dumb).
# use the same UART for telemetry
#
echo "[init] startup done, exiting."
echo "[init] startup done"
exit

1
ROMFS/scripts/rc.sensors
View File

@ -8,6 +8,7 @@
#
ms5611 start
adc start
if mpu6000 start
then

8
ROMFS/scripts/rcS
View File

@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ]
then
echo "[init] reading /fs/microsd/etc/rc"
sh /fs/microsd/etc/rc
else
echo "[init] script /fs/microsd/etc/rc not present"
fi
# Also consider rc.txt files
if [ -f /fs/microsd/etc/rc.txt ]
then
echo "[init] reading /fs/microsd/etc/rc.txt"
sh /fs/microsd/etc/rc.txt
fi
#

19
Tools/fix_code_style_ubuntu.sh Executable file
View File

@ -0,0 +1,19 @@
#!/bin/sh
astyle \
--style=linux \
--indent=force-tab=8 \
--indent-cases \
--indent-preprocessor \
--break-blocks=all \
--pad-oper \
--pad-header \
--unpad-paren \
--keep-one-line-blocks \
--keep-one-line-statements \
--align-pointer=name \
--suffix=none \
--lineend=linux \
$*
#--ignore-exclude-errors-x \
#--exclude=EASTL \
#--align-reference=name \

71
apps/ChangeLog.txt
View File

@ -33,7 +33,7 @@
6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org>
* apps/interpreter: Add a directory to hold interpreters. The Pascal add-
* apps/interpreter: Add a directory to hold interpreters. The Pascal add-
on module now installs and builds under this directory.
* apps/interpreter/ficl: Added logic to build Ficl (the "Forth Inspired
Command Language"). See http://ficl.sourceforge.net/.
@ -349,7 +349,7 @@
* apps/NxWidgets/Kconfig: Add option to turn on the memory monitor
feature of the NxWidgets/NxWM unit tests.
6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
6.23 2012-11-05 Gregory Nutt <gnutt@nuttx.org>
* vsn: Moved all NSH commands from vsn/ to system/. Deleted the vsn/
directory.
@ -368,3 +368,70 @@
recent check-ins (Darcy Gong).
* apps/netutils/webclient/webclient.c: Fix another but that I introduced
when I was trying to add correct handling for loss of connection (Darcy Gong)
* apps/nshlib/nsh_telnetd.c: Add support for login to Telnet session via
username and password (Darcy Gong).
* apps/netutils/resolv/resolv.c (and files using the DNS resolver): Various
DNS address resolution improvements from Darcy Gong.
* apps/nshlib/nsh_netcmds.c: The ping command now passes a maximum round
trip time to uip_icmpping(). This allows pinging of hosts on complex
networks where the ICMP ECHO round trip time may exceed the ping interval.
* apps/examples/nxtext/nxtext_main.c: Fix bad conditional compilation
when CONFIG_NX_KBD is not defined. Submitted by Petteri Aimonen.
* apps/examples/nximage/nximage_main.c: Add a 5 second delay after the
NX logo is presented so that there is time for the image to be verified.
Suggested by Petteri Aimonen.
* apps/Makefile: Small change that reduces the number of shell invocations
by one (Mike Smith).
* apps/examples/elf: Test example for the ELF loader.
* apps/examples/elf: The ELF module test example appears fully functional.
* apps/netutils/json: Add a snapshot of the cJSON project. Contributed by
Darcy Gong.
* apps/examples/json: Test example for cJSON from Darcy Gong
* apps/nshlib/nsh_netinit.c: Fix static IP DNS problem (Darcy Gong)
* apps/netutils/resolv/resolv.c: DNS fixes from Darcy Gong.
* COPYING: Licensing information added.
* apps/netutils/codec and include/netutils/urldecode.h, base64.h, and md5.h:
A port of the BASE46, MD5 and URL CODEC library from Darcy Gong.
* nsnlib/nsh_codeccmd.c: NSH commands to use the CODEC library.
Contributed by Darcy Gong.
* apps/examples/wgetjson: Test example contributed by Darcy Gong
* apps/examples/cxxtest: A test for the uClibc++ library provided by
Qiang Yu and the RGMP team.
* apps/netutils/webclient, apps/netutils.codes, and apps/examples/wgetjson:
Add support for wget POST interface. Contributed by Darcy Gong.
* apps/examples/relays: A relay example contributed by Darcy Gong.
* apps/nshlib/nsh_netcmds: Add ifup and ifdown commands (from Darcy
Gong).
* apps/nshlib/nsh_netcmds: Extend the ifconfig command so that it
supports setting IP addresses, network masks, name server addresses,
and hardware address (from Darcy Gong).
6.24 2012-12-20 Gregory Nutt <gnutt@nuttx.org>
* apps/examples/ostest/roundrobin.c: Replace large tables with
algorithmic prime number generation. This allows the roundrobin
test to run on platforms with minimal SRAM (Freddie Chopin).
* apps/nshlib/nsh_dbgcmds.c: Add hexdump command to dump the contents
of a file (or character device) to the console Contributed by Petteri
Aimonen.
* apps/examples/modbus: Fixes from Freddie Chopin
* apps/examples/modbus/Kconfig: Kconfig logic for FreeModBus contributed
by Freddie Chopin.
* Makefile, */Makefile: Various fixes for Windows native build. Now uses
make foreach loops instead of shell loops.
* apps/examples/elf/test/*/Makefile: OSX doesn't support install -D, use
mkdir -p then install without the -D. From Mike Smith.
* apps/examples/relays/Makefile: Reduced stack requirement (Darcy Gong).
* apps/nshlib and apps/netutils/dhcpc: Extend the NSH ifconfig command plus
various DHCPC improvements(Darcy Gong).
* apps/nshlib/nsh_apps.c: Fix compilation errors when CONFIG_NSH_DISABLEBG=y.
From Freddie Chopin.
* Rename CONFIG_PCODE and CONFIG_FICL as CONFIG_INTERPRETERS_PCODE and
CONFIG_INTERPRETERS_FICL for consistency with other configuration naming.
* apps/examples/keypadtest: A keypad test example contributed by Denis
Carikli.
* apps/examples/elf and nxflat: If CONFIG_BINFMT_EXEPATH is defined, these
tests will now use a relative path to the program and expect the binfmt/
logic to find the absolute path to the program using the PATH variable.
6.25 2013-xx-xx Gregory Nutt <gnutt@nuttx.org>

74
apps/Makefile
View File

@ -107,18 +107,23 @@ endif
# Create the list of available applications (INSTALLED_APPS)
define ADD_BUILTIN
INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi}
INSTALLED_APPS += $(if $(wildcard $1$(DELIM)Makefile),$1,)
endef
$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
# EXTERNAL_APPS is used to add out of tree apps to the build
INSTALLED_APPS += $(EXTERNAL_APPS)
# The external/ directory may also be added to the INSTALLED_APPS. But there
# is no external/ directory in the repository. Rather, this directory may be
# provided by the user (possibly as a symbolic link) to add libraries and
# applications to the standard build from the repository.
INSTALLED_APPS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
SUBDIRS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
EXTERNAL_DIR := $(dir $(wildcard external$(DELIM)Makefile))
INSTALLED_APPS += $(EXTERNAL_DIR)
SUBDIRS += $(EXTERNAL_DIR)
# The final build target
@ -130,48 +135,81 @@ all: $(BIN)
.PHONY: $(INSTALLED_APPS) context depend clean distclean
$(INSTALLED_APPS):
@$(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)";
$(Q) $(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
$(BIN): $(INSTALLED_APPS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $@, $${obj}); \
done ; )
.context:
@for dir in $(INSTALLED_APPS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
if exist %%G\.context del /f /q %%G\.context \
$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context \
)
else
$(Q) for dir in $(INSTALLED_APPS) ; do \
rm -f $$dir/.context ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \
done
@touch $@
endif
$(Q) touch $@
context: .context
.depend: context Makefile $(SRCS)
@for dir in $(INSTALLED_APPS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
if exist %%G\.depend del /f /q %%G\.depend \
$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend \
)
else
$(Q) for dir in $(INSTALLED_APPS) ; do \
rm -f $$dir/.depend ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \
$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \
done
@touch $@
endif
$(Q) touch $@
depend: .depend
clean:
@for dir in $(SUBDIRS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(SUBDIRS)) do ( \
$(MAKE) -C %%G clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
)
else
$(Q) for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
@rm -f $(BIN) *~ .*.swp *.o
endif
$(call DELFILE, $(BIN))
$(call CLEAN)
distclean: # clean
@for dir in $(SUBDIRS) ; do \
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
$(Q) for %%G in ($(SUBDIRS)) do ( \
$(MAKE) -C %%G distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
)
$(call DELFILE, .config)
$(call DELFILE, .context)
$(call DELFILE, .depend)
$(Q) ( if exist external ( \
echo ********************************************************" \
echo * The external directory/link must be removed manually *" \
echo ********************************************************" \
)
else
$(Q) for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
@rm -f .config .context .depend
@( if [ -e external ]; then \
$(call DELFILE, .config)
$(call DELFILE, .context)
$(call DELFILE, .depend)
$(Q) ( if [ -e external ]; then \
echo "********************************************************"; \
echo "* The external directory/link must be removed manually *"; \
echo "********************************************************"; \
fi; \
)
endif

78
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_main.c
*
*
* Extended Kalman Filter for Attitude Estimation.
*/
@ -79,18 +79,18 @@ static float dt = 0.005f;
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< states */
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float x_aposteriori[12];
static float P_aposteriori[144];
@ -99,9 +99,9 @@ static float P_aposteriori[144];
static float euler[3] = {0.0f, 0.0f, 0.0f};
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
@ -123,6 +123,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@ -131,7 +132,7 @@ usage(const char *reason)
* The attitude_estimator_ekf 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().
*/
@ -150,11 +151,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
thread_should_exit = false;
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12000,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
@ -166,9 +167,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tattitude_estimator_ekf app is running\n");
} else {
printf("\tattitude_estimator_ekf app not started\n");
}
exit(0);
}
@ -235,7 +238,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* advertise debug value */
// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
// orb_advert_t pub_dbg = -1;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
@ -263,8 +266,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
struct pollfd fds[2] = {
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 2, 1000);
@ -273,10 +276,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
if (!state.flag_hil_enabled) {
fprintf(stderr,
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
} else {
/* only update parameters if they changed */
@ -308,6 +313,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
}
} else {
perf_begin(ekf_loop_perf);
@ -336,6 +342,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp;
}
z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2];
@ -347,6 +354,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.timestamp;
}
z_k[6] = raw.magnetometer_ga[0];
z_k[7] = raw.magnetometer_ga[1];
z_k[8] = raw.magnetometer_ga[2];
@ -368,8 +376,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized && dt < 0.05f && dt > 0.005f)
{
if (!const_initialized && dt < 0.05f && dt > 0.005f) {
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
@ -395,13 +402,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
}
uint64_t timing_start = hrt_absolute_time();
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
@ -413,9 +422,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* send out */
att.timestamp = raw.timestamp;
att.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2];
// XXX Apply the same transformation to the rotation matrix
att.roll = euler[0] - ekf_params.roll_off;
att.pitch = euler[1] - ekf_params.pitch_off;
att.yaw = euler[2] - ekf_params.yaw_off;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
@ -431,6 +442,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
// Broadcast
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
} else {
warnx("NaN in roll/pitch/yaw estimate!");
}

20
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_params.c
*
*
* Parameters for EKF filter
*/
@ -65,13 +65,17 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
/* magnetometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
{
@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
h->r7 = param_find("EKF_ATT_R7");
h->r8 = param_find("EKF_ATT_R8");
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r7, &(p->r[7]));
param_get(h->r8, &(p->r[8]));
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}

6
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
View File

@ -35,7 +35,7 @@
/*
* @file attitude_estimator_ekf_params.h
*
*
* Parameters for EKF filter
*/
@ -44,11 +44,15 @@
struct attitude_estimator_ekf_params {
float r[9];
float q[12];
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_ekf_param_handles {
param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
param_t roll_off, pitch_off, yaw_off;
};
/**

283
apps/commander/calibration_routines.c
View File

@ -45,172 +45,175 @@
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
{
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float x_sumplain = 0.0f;
float x_sumsq = 0.0f;
float x_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float y_sumplain = 0.0f;
float y_sumsq = 0.0f;
float y_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float z_sumplain = 0.0f;
float z_sumsq = 0.0f;
float z_sumcube = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float xy_sum = 0.0f;
float xz_sum = 0.0f;
float yz_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
float x2y_sum = 0.0f;
float x2z_sum = 0.0f;
float y2x_sum = 0.0f;
float y2z_sum = 0.0f;
float z2x_sum = 0.0f;
float z2y_sum = 0.0f;
for (unsigned int i = 0; i < size; i++) {
for (unsigned int i = 0; i < size; i++) {
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
float x2 = x[i] * x[i];
float y2 = y[i] * y[i];
float z2 = z[i] * z[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
x_sumplain += x[i];
x_sumsq += x2;
x_sumcube += x2 * x[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
y_sumplain += y[i];
y_sumsq += y2;
y_sumcube += y2 * y[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
z_sumplain += z[i];
z_sumsq += z2;
z_sumcube += z2 * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
xy_sum += x[i] * y[i];
xz_sum += x[i] * z[i];
yz_sum += y[i] * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
x2y_sum += x2 * y[i];
x2z_sum += x2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
y2x_sum += y2 * x[i];
y2z_sum += y2 * z[i];
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
z2x_sum += z2 * x[i];
z2y_sum += z2 * y[i];
}
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
//
//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
//
// P is a structure that has been computed with the data earlier.
// P.npoints is the number of elements; the length of X,Y,Z are identical.
// P's members are logically named.
//
// X[n] is the x component of point n
// Y[n] is the y component of point n
// Z[n] is the z component of point n
//
// A is the x coordiante of the sphere
// B is the y coordiante of the sphere
// C is the z coordiante of the sphere
// Rsq is the radius squared of the sphere.
//
//This method should converge; maybe 5-100 iterations or more.
//
float x_sum = x_sumplain / size; //sum( X[n] )
float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
float y_sum = y_sumplain / size; //sum( Y[n] )
float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
float z_sum = z_sumplain / size; //sum( Z[n] )
float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
float XY = xy_sum / size; //sum( X[n] * Y[n] )
float XZ = xz_sum / size; //sum( X[n] * Z[n] )
float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Reduction of multiplications
float F0 = x_sum2 + y_sum2 + z_sum2;
float F1 = 0.5f * F0;
float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//Set initial conditions:
float A = x_sum;
float B = y_sum;
float C = z_sum;
//First iteration computation:
float A2 = A*A;
float B2 = B*B;
float C2 = C*C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
//First iteration computation:
float A2 = A * A;
float B2 = B * B;
float C2 = C * C;
float QS = A2 + B2 + C2;
float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
//Set initial conditions:
float Rsq = F0 + QB + QS;
//Set initial conditions:
float Rsq = F0 + QB + QS;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
float aA,aB,aC,nA,nB,nC,dA,dB,dC;
//First iteration computation:
float Q0 = 0.5f * (QS - Rsq);
float Q1 = F1 + Q0;
float Q2 = 8.0f * (QS - Rsq + QB + F0);
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
while( n < max_iterations ){
n++;
//Iterate N times, ignore stop condition.
int n = 0;
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
while (n < max_iterations) {
n++;
//Compute next iteration
nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
//Compute denominator:
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A*A;
B2 = B*B;
C2 = C*C;
QS = A2 + B2 + C2;
QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * ( QS - Rsq + QB + F0 );
}
//Check for stop condition
dA = (nA - A);
dB = (nB - B);
dC = (nC - C);
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
return 0;
//Compute next iteration's values
A = nA;
B = nB;
C = nC;
A2 = A * A;
B2 = B * B;
C2 = C * C;
QS = A2 + B2 + C2;
QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
Rsq = F0 + QB + QS;
Q0 = 0.5f * (QS - Rsq);
Q1 = F1 + Q0;
Q2 = 8.0f * (QS - Rsq + QB + F0);
}
*sphere_x = A;
*sphere_y = B;
*sphere_z = C;
*sphere_radius = sqrtf(Rsq);
return 0;
}

2
apps/commander/calibration_routines.h
View File

@ -58,4 +58,4 @@
* @return 0 on success, 1 on failure
*/
int sphere_fit_least_squares(const float x[], const float y[], const float z[],
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);

920
apps/commander/commander.c
View File

File diff suppressed because it is too large Load Diff

3
apps/commander/commander.h
View File

@ -52,4 +52,7 @@
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
void tune_confirm(void);
void tune_error(void);
#endif /* COMMANDER_H_ */

221
apps/commander/state_machine_helper.c
View File

@ -50,7 +50,7 @@
#include "state_machine_helper.h"
static const char* system_state_txt[] = {
static const char *system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT",
"SYSTEM_STATE_STANDBY",
"SYSTEM_STATE_GROUND_READY",
@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
case SYSTEM_STATE_MISSION_ABORT: {
/* Indoor or outdoor */
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
// } else {
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
warnx("EMERGENCY LANDING!\n");
mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
break;
case SYSTEM_STATE_EMCY_CUTOFF:
@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
warnx("EMERGENCY MOTOR CUTOFF!\n");
mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
break;
case SYSTEM_STATE_GROUND_ERROR:
@ -114,36 +114,41 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* prevent actuators from arming */
current_status->flag_system_armed = false;
fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
warnx("GROUND ERROR, locking down propulsion system\n");
mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
break;
case SYSTEM_STATE_PREFLIGHT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
}
break;
case SYSTEM_STATE_REBOOT:
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
|| current_status->flag_hil_enabled) {
invalid_state = false;
/* set system flags according to state */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
usleep(500000);
up_systemreset();
/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
} else {
invalid_state = true;
mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
}
break;
case SYSTEM_STATE_STANDBY:
@ -152,7 +157,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* standby enforces disarmed */
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
break;
case SYSTEM_STATE_GROUND_READY:
@ -162,7 +167,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* ground ready has motors / actuators armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
break;
case SYSTEM_STATE_AUTO:
@ -172,7 +177,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* auto is airborne and in auto mode, motors armed */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
break;
case SYSTEM_STATE_STABILIZED:
@ -180,7 +185,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
break;
case SYSTEM_STATE_MANUAL:
@ -188,7 +193,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
/* set system flags according to state */
current_status->flag_system_armed = true;
mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
break;
default:
@ -202,14 +207,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
publish_armed_status(current_status);
ret = OK;
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
ret = ERROR;
}
return ret;
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
/* publish the new state */
current_status->counter++;
current_status->timestamp = hrt_absolute_time();
@ -217,9 +225,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
/* assemble state vector based on flag values */
if (current_status->flag_control_rates_enabled) {
current_status->onboard_control_sensors_present |= 0x400;
} else {
current_status->onboard_control_sensors_present &= ~0x400;
}
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
@ -232,10 +242,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
}
void publish_armed_status(const struct vehicle_status_s *current_status) {
void publish_armed_status(const struct vehicle_status_s *current_status)
{
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* lock down actuators if required, only in HIL */
@ -250,19 +261,19 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
warnx("EMERGENCY HANDLER\n");
/* Depending on the current state go to one of the error states */
if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
// DO NOT abort mission
//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
} else {
fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
warnx("Unknown system state: #%d\n", current_status->state_machine);
}
}
@ -272,7 +283,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
} else {
//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
}
}
@ -497,7 +508,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
printf("[commander] arming\n");
printf("[cmd] arming\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
}
}
@ -505,11 +516,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
printf("[commander] going standby\n");
printf("[cmd] going standby\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] MISSION ABORT!\n");
printf("[cmd] MISSION ABORT!\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
}
}
@ -518,54 +529,139 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
/* set behaviour based on airframe */
if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
/* assuming a rotary wing, set to SAS */
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
} else {
/* assuming a fixed wing, set to direct pass-through */
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
current_status->flag_control_attitude_enabled = false;
current_status->flag_control_rates_enabled = false;
}
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] manual mode\n");
printf("[cmd] manual mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
}
}
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
int old_mode = current_status->flight_mode;
int old_manual_control_mode = current_status->manual_control_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
current_status->flag_control_manual_enabled = true;
if (old_mode != current_status->flight_mode ||
old_manual_control_mode != current_status->manual_control_mode) {
printf("[cmd] att stabilized mode\n");
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
}
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
tune_error();
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
printf("[commander] stabilized mode\n");
printf("[cmd] position guided mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (!current_status->flag_vector_flight_mode_ok) {
mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
return;
}
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
printf("[commander] auto mode\n");
printf("[cmd] auto mode\n");
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = false;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = true;
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
}
}
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{
printf("[commander] Requested new mode: %d\n", (int)mode);
uint8_t ret = 1;
/* Switch on HIL if in standby and not already in HIL mode */
if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
current_status->flag_system_armed) {
mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
} else {
mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
}
}
/* switch manual / auto */
if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
}
/* vehicle is disarmed, mode requests arming */
if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
/* only arm in standby state */
@ -573,7 +669,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
ret = OK;
printf("[commander] arming due to command request\n");
printf("[cmd] arming due to command request\n");
}
}
@ -583,26 +679,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
ret = OK;
printf("[commander] disarming due to command request\n");
printf("[cmd] disarming due to command request\n");
}
}
/* Switch on HIL if in standby and not already in HIL mode */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
}
/* NEVER actually switch off HIL without reboot */
if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
ret = ERROR;
}
@ -625,9 +709,12 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
case SYSTEM_STATE_REBOOT:
printf("try to reboot\n");
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
if (current_system_state == SYSTEM_STATE_STANDBY
|| current_system_state == SYSTEM_STATE_PREFLIGHT
|| current_status->flag_hil_enabled) {
printf("system will reboot\n");
//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
mavlink_log_critical(mavlink_fd, "Rebooting..");
usleep(200000);
do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
ret = 0;
}

9
apps/commander/state_machine_helper.h
View File

@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
*/
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is guided
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is auto
*

46
apps/controllib/Makefile Normal file
View File

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

210
apps/controllib/block/Block.cpp Normal file
View File

@ -0,0 +1,210 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Block.cpp
*
* Controller library code
*/
#include <math.h>
#include <string.h>
#include <stdio.h>
#include "Block.hpp"
#include "BlockParam.hpp"
#include "UOrbSubscription.hpp"
#include "UOrbPublication.hpp"
namespace control
{
Block::Block(SuperBlock *parent, const char *name) :
_name(name),
_parent(parent),
_dt(0),
_subscriptions(),
_params()
{
if (getParent() != NULL) {
getParent()->getChildren().add(this);
}
}
void Block::getName(char *buf, size_t n)
{
if (getParent() == NULL) {
strncpy(buf, _name, n);
} else {
char parentName[blockNameLengthMax];
getParent()->getName(parentName, n);
if (!strcmp(_name, "")) {
strncpy(buf, parentName, blockNameLengthMax);
} else {
snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
}
}
}
void Block::updateParams()
{
BlockParamBase *param = getParams().getHead();
int count = 0;
while (param != NULL) {
if (count++ > maxParamsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max params for block: %s\n", name);
break;
}
//printf("updating param: %s\n", param->getName());
param->update();
param = param->getSibling();
}
}
void Block::updateSubscriptions()
{
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max subscriptions for block: %s\n", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
void Block::updatePublications()
{
UOrbPublicationBase *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
if (count++ > maxPublicationsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max publications for block: %s\n", name);
break;
}
pub->update();
pub = pub->getSibling();
}
}
void SuperBlock::setDt(float dt)
{
Block::setDt(dt);
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->setDt(dt);
child = child->getSibling();
}
}
void SuperBlock::updateChildParams()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateParams();
child = child->getSibling();
}
}
void SuperBlock::updateChildSubscriptions()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updateSubscriptions();
child = child->getSibling();
}
}
void SuperBlock::updateChildPublications()
{
Block *child = getChildren().getHead();
int count = 0;
while (child != NULL) {
if (count++ > maxChildrenPerBlock) {
char name[40];
getName(name, 40);
printf("exceeded max children for block: %s\n", name);
break;
}
child->updatePublications();
child = child->getSibling();
}
}
} // namespace control

131
apps/controllib/block/Block.hpp Normal file
View File

@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Block.h
*
* Controller library code
*/
#pragma once
#include <stdint.h>
#include <inttypes.h>
#include "List.hpp"
namespace control
{
static const uint16_t maxChildrenPerBlock = 100;
static const uint16_t maxParamsPerBlock = 100;
static const uint16_t maxSubscriptionsPerBlock = 100;
static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class BlockParamBase;
class UOrbSubscriptionBase;
class UOrbPublicationBase;
class SuperBlock;
/**
*/
class __EXPORT Block :
public ListNode<Block *>
{
public:
friend class BlockParamBase;
// methods
Block(SuperBlock *parent, const char *name);
void getName(char *name, size_t n);
virtual ~Block() {};
virtual void updateParams();
virtual void updateSubscriptions();
virtual void updatePublications();
virtual void setDt(float dt) { _dt = dt; }
// accessors
float getDt() { return _dt; }
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
List<UOrbPublicationBase *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
List<UOrbSubscriptionBase *> _subscriptions;
List<UOrbPublicationBase *> _publications;
List<BlockParamBase *> _params;
};
class __EXPORT SuperBlock :
public Block
{
public:
friend class Block;
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children() {
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams() {
Block::updateParams();
if (getChildren().getHead() != NULL) updateChildParams();
}
virtual void updateSubscriptions() {
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) updateChildSubscriptions();
}
virtual void updatePublications() {
Block::updatePublications();
if (getChildren().getHead() != NULL) updateChildPublications();
}
protected:
// methods
List<Block *> & getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
void updateChildPublications();
// attributes
List<Block *> _children;
};
} // namespace control

77
apps/controllib/block/BlockParam.cpp Normal file
View File

@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Blockparam.cpp
*
* Controller library code
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "BlockParam.hpp"
namespace control
{
BlockParamBase::BlockParamBase(Block *parent, const char *name) :
_handle(PARAM_INVALID)
{
char fullname[blockNameLengthMax];
if (parent == NULL) {
strncpy(fullname, name, blockNameLengthMax);
} else {
char parentName[blockNameLengthMax];
parent->getName(parentName, blockNameLengthMax);
if (!strcmp(name, "")) {
strncpy(fullname, parentName, blockNameLengthMax);
} else {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
}
parent->getParams().add(this);
}
_handle = param_find(fullname);
if (_handle == PARAM_INVALID)
printf("error finding param: %s\n", fullname);
};
} // namespace control

85
apps/controllib/block/BlockParam.hpp Normal file
View File

@ -0,0 +1,85 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file BlockParam.h
*
* Controller library code
*/
#pragma once
#include <systemlib/param/param.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
/**
* A base class for block params that enables traversing linked list.
*/
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
{
public:
BlockParamBase(Block *parent, const char *name);
virtual ~BlockParamBase() {};
virtual void update() = 0;
const char *getName() { return param_name(_handle); }
protected:
param_t _handle;
};
/**
* Parameters that are tied to blocks for updating and nameing.
*/
template<class T>
class __EXPORT BlockParam : public BlockParamBase
{
public:
BlockParam(Block *block, const char *name) :
BlockParamBase(block, name),
_val() {
update();
}
T get() { return _val; }
void set(T val) { _val = val; }
void update() {
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
}
protected:
T _val;
};
} // namespace control

71
apps/controllib/block/List.hpp Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Node.h
*
* A node of a linked list.
*/
#pragma once
template<class T>
class __EXPORT ListNode
{
public:
ListNode() : _sibling(NULL) {
}
void setSibling(T sibling) { _sibling = sibling; }
T getSibling() { return _sibling; }
T get() {
return _sibling;
}
protected:
T _sibling;
};
template<class T>
class __EXPORT List
{
public:
List() : _head() {
}
void add(T newNode) {
newNode->setSibling(getHead());
setHead(newNode);
}
T getHead() { return _head; }
private:
void setHead(T &head) { _head = head; }
T _head;
};

39
apps/controllib/block/UOrbPublication.cpp Normal file
View File

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

118
apps/controllib/block/UOrbPublication.hpp Normal file
View File

@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbPublication.h
*
*/
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
class Block;
/**
* Base publication warapper class, used in list traversal
* of various publications.
*/
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
{
public:
UOrbPublicationBase(
List<UOrbPublicationBase *> * list,
const struct orb_metadata *meta) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
}
void update() {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
}
virtual void *getDataVoidPtr() = 0;
virtual ~UOrbPublicationBase() {
orb_unsubscribe(getHandle());
}
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
void setHandle(orb_advert_t handle) { _handle = handle; }
const struct orb_metadata *_meta;
orb_advert_t _handle;
};
/**
* UOrb Publication wrapper class
*/
template<class T>
class UOrbPublication :
public T, // this must be first!
public UOrbPublicationBase
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
*/
UOrbPublication(
List<UOrbPublicationBase *> * list,
const struct orb_metadata *meta) :
T(), // initialize data structure to zero
UOrbPublicationBase(list, meta) {
// It is important that we call T()
// before we publish the data, so we
// call this here instead of the base class
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
}
virtual ~UOrbPublication() {}
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr() { return (void *)(T *)(this); }
};
} // namespace control

51
apps/controllib/block/UOrbSubscription.cpp Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbSubscription.cpp
*
*/
#include "UOrbSubscription.hpp"
namespace control
{
bool __EXPORT UOrbSubscriptionBase::updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
} // namespace control

137
apps/controllib/block/UOrbSubscription.hpp Normal file
View File

@ -0,0 +1,137 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file UOrbSubscription.h
*
*/
#pragma once
#include <uORB/uORB.h>
#include "Block.hpp"
#include "List.hpp"
namespace control
{
class Block;
/**
* Base subscription warapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT UOrbSubscriptionBase :
public ListNode<control::UOrbSubscriptionBase *>
{
public:
// methods
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
*/
UOrbSubscriptionBase(
List<UOrbSubscriptionBase *> * list,
const struct orb_metadata *meta) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
}
bool updated();
void update() {
if (updated()) {
orb_copy(_meta, _handle, getDataVoidPtr());
}
}
virtual void *getDataVoidPtr() = 0;
virtual ~UOrbSubscriptionBase() {
orb_unsubscribe(_handle);
}
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
// accessors
void setHandle(int handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _handle;
};
/**
* UOrb Subscription wrapper class
*/
template<class T>
class __EXPORT UOrbSubscription :
public T, // this must be first!
public UOrbSubscriptionBase
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param interval The minimum interval in milliseconds between updates
*/
UOrbSubscription(
List<UOrbSubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval) :
T(), // initialize data structure to zero
UOrbSubscriptionBase(list, meta) {
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
/**
* Deconstructor
*/
virtual ~UOrbSubscription() {}
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr() { return (void *)(T *)(this); }
T getData() { return T(*this); }
};
} // namespace control

486
apps/controllib/blocks.cpp Normal file
View File

@ -0,0 +1,486 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file blocks.cpp
*
* Controller library code
*/
#include <math.h>
#include <stdio.h>
#include "blocks.hpp"
namespace control
{
int basicBlocksTest()
{
blockLimitTest();
blockLimitSymTest();
blockLowPassTest();
blockHighPassTest();
blockIntegralTest();
blockIntegralTrapTest();
blockDerivativeTest();
blockPTest();
blockPITest();
blockPDTest();
blockPIDTest();
blockOutputTest();
blockRandUniformTest();
blockRandGaussTest();
return 0;
}
float BlockLimit::update(float input)
{
if (input > getMax()) {
input = _max.get();
} else if (input < getMin()) {
input = getMin();
}
return input;
}
int blockLimitTest()
{
printf("Test BlockLimit\t\t\t: ");
BlockLimit limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(-1.0f, limit.getMin()));
ASSERT(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
float BlockLimitSym::update(float input)
{
if (input > getMax()) {
input = _max.get();
} else if (input < -getMax()) {
input = -getMax();
}
return input;
}
int blockLimitSymTest()
{
printf("Test BlockLimitSym\t\t: ");
BlockLimitSym limit(NULL, "TEST");
// initial state
ASSERT(equal(1.0f, limit.getMax()));
ASSERT(equal(0.0f, limit.getDt()));
// update
ASSERT(equal(-1.0f, limit.update(-2.0f)));
ASSERT(equal(1.0f, limit.update(2.0f)));
ASSERT(equal(0.0f, limit.update(0.0f)));
printf("PASS\n");
return 0;
}
float BlockLowPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
return getState();
}
int blockLowPassTest()
{
printf("Test BlockLowPass\t\t: ");
BlockLowPass lowPass(NULL, "TEST_LP");
// test initial state
ASSERT(equal(10.0f, lowPass.getFCut()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
float BlockHighPass::update(float input)
{
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = 1 / (1 + b);
setY(a * (getY() + input - getU()));
setU(input);
return getY();
}
int blockHighPassTest()
{
printf("Test BlockHighPass\t\t: ");
BlockHighPass highPass(NULL, "TEST_HP");
// test initial state
ASSERT(equal(10.0f, highPass.getFCut()));
ASSERT(equal(0.0f, highPass.getU()));
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.getDt()));
// set dt
highPass.setDt(0.1f);
ASSERT(equal(0.1f, highPass.getDt()));
// set state
highPass.setU(1.0f);
ASSERT(equal(1.0f, highPass.getU()));
highPass.setY(1.0f);
ASSERT(equal(1.0f, highPass.getY()));
// test update
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
highPass.update(2.0f);
}
ASSERT(equal(0.0f, highPass.getY()));
ASSERT(equal(0.0f, highPass.update(2.0f)));
printf("PASS\n");
return 0;
}
float BlockIntegral::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() + input * getDt()));
return getY();
}
int blockIntegralTest()
{
printf("Test BlockIntegral\t\t: ");
BlockIntegral integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setY(0.1f);
ASSERT(equal(0.2f, integral.update(1.0)));
ASSERT(equal(0.2f, integral.getY()));
printf("PASS\n");
return 0;
}
float BlockIntegralTrap::update(float input)
{
// trapezoidal integration
setY(_limit.update(getY() +
(getU() + input) / 2.0f * getDt()));
setU(input);
return getY();
}
int blockIntegralTrapTest()
{
printf("Test BlockIntegralTrap\t\t: ");
BlockIntegralTrap integral(NULL, "TEST_I");
// test initial state
ASSERT(equal(1.0f, integral.getMax()));
ASSERT(equal(0.0f, integral.getDt()));
// set dt
integral.setDt(0.1f);
ASSERT(equal(0.1f, integral.getDt()));
// set U
integral.setU(1.0f);
ASSERT(equal(1.0f, integral.getU()));
// set Y
integral.setY(0.9f);
ASSERT(equal(0.9f, integral.getY()));
// test exceed max
for (int i = 0; i < 100; i++) {
integral.update(1.0f);
}
ASSERT(equal(1.0f, integral.update(1.0f)));
// test exceed min
integral.setU(-1.0f);
integral.setY(-0.9f);
ASSERT(equal(-0.9f, integral.getY()));
for (int i = 0; i < 100; i++) {
integral.update(-1.0f);
}
ASSERT(equal(-1.0f, integral.update(-1.0f)));
// test update
integral.setU(2.0f);
integral.setY(0.1f);
ASSERT(equal(0.25f, integral.update(1.0)));
ASSERT(equal(0.25f, integral.getY()));
ASSERT(equal(1.0f, integral.getU()));
printf("PASS\n");
return 0;
}
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
setU(input);
return output;
}
int blockDerivativeTest()
{
printf("Test BlockDerivative\t\t: ");
BlockDerivative derivative(NULL, "TEST_D");
// test initial state
ASSERT(equal(0.0f, derivative.getU()));
ASSERT(equal(10.0f, derivative.getLP()));
// set dt
derivative.setDt(0.1f);
ASSERT(equal(0.1f, derivative.getDt()));
// set U
derivative.setU(1.0f);
ASSERT(equal(1.0f, derivative.getU()));
// test update
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
ASSERT(equal(2.0f, derivative.getU()));
printf("PASS\n");
return 0;
}
int blockPTest()
{
printf("Test BlockP\t\t\t: ");
BlockP blockP(NULL, "TEST_P");
// test initial state
ASSERT(equal(0.2f, blockP.getKP()));
ASSERT(equal(0.0f, blockP.getDt()));
// set dt
blockP.setDt(0.1f);
ASSERT(equal(0.1f, blockP.getDt()));
// test update
ASSERT(equal(0.4f, blockP.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPITest()
{
printf("Test BlockPI\t\t\t: ");
BlockPI blockPI(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPI.getKP()));
ASSERT(equal(0.1f, blockPI.getKI()));
ASSERT(equal(0.0f, blockPI.getDt()));
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
// set dt
blockPI.setDt(0.1f);
ASSERT(equal(0.1f, blockPI.getDt()));
// set integral state
blockPI.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
ASSERT(equal(0.43f, blockPI.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPDTest()
{
printf("Test BlockPD\t\t\t: ");
BlockPD blockPD(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPD.getKP()));
ASSERT(equal(0.01f, blockPD.getKD()));
ASSERT(equal(0.0f, blockPD.getDt()));
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
// set dt
blockPD.setDt(0.1f);
ASSERT(equal(0.1f, blockPD.getDt()));
// set derivative state
blockPD.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
// test update
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockPIDTest()
{
printf("Test BlockPID\t\t\t: ");
BlockPID blockPID(NULL, "TEST");
// test initial state
ASSERT(equal(0.2f, blockPID.getKP()));
ASSERT(equal(0.1f, blockPID.getKI()));
ASSERT(equal(0.01f, blockPID.getKD()));
ASSERT(equal(0.0f, blockPID.getDt()));
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
// set dt
blockPID.setDt(0.1f);
ASSERT(equal(0.1f, blockPID.getDt()));
// set derivative state
blockPID.getDerivative().setU(1.0f);
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
// set integral state
blockPID.getIntegral().setY(0.1f);
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
// test update
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
printf("PASS\n");
return 0;
}
int blockOutputTest()
{
printf("Test BlockOutput\t\t: ");
BlockOutput blockOutput(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockOutput.getDt()));
ASSERT(equal(0.5f, blockOutput.get()));
ASSERT(equal(-1.0f, blockOutput.getMin()));
ASSERT(equal(1.0f, blockOutput.getMax()));
// test update below min
blockOutput.update(-2.0f);
ASSERT(equal(-1.0f, blockOutput.get()));
// test update above max
blockOutput.update(2.0f);
ASSERT(equal(1.0f, blockOutput.get()));
// test trim
blockOutput.update(0.0f);
ASSERT(equal(0.5f, blockOutput.get()));
printf("PASS\n");
return 0;
}
int blockRandUniformTest()
{
srand(1234);
printf("Test BlockRandUniform\t\t: ");
BlockRandUniform blockRandUniform(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandUniform.getDt()));
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
ASSERT(equal(1.0f, blockRandUniform.getMax()));
// test update
int n = 10000;
float mean = blockRandUniform.update();
// recursive mean algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandUniform.update();
mean += (val - mean) / i;
ASSERT(val <= blockRandUniform.getMax());
ASSERT(val >= blockRandUniform.getMin());
}
ASSERT(equal(mean, (blockRandUniform.getMin() +
blockRandUniform.getMax()) / 2, 1e-1));
printf("PASS\n");
return 0;
}
int blockRandGaussTest()
{
srand(1234);
printf("Test BlockRandGauss\t\t: ");
BlockRandGauss blockRandGauss(NULL, "TEST");
// test initial state
ASSERT(equal(0.0f, blockRandGauss.getDt()));
ASSERT(equal(1.0f, blockRandGauss.getMean()));
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
// test update
int n = 10000;
float mean = blockRandGauss.update();
float sum = 0;
// recursive mean, stdev algorithm from Knuth
for (int i = 2; i < n + 1; i++) {
float val = blockRandGauss.update();
float newMean = mean + (val - mean) / i;
sum += (val - mean) * (val - newMean);
mean = newMean;
}
float stdDev = sqrt(sum / (n - 1));
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
printf("PASS\n");
return 0;
}
} // namespace control

494
apps/controllib/blocks.hpp Normal file
View File

@ -0,0 +1,494 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file blocks.h
*
* Controller library code
*/
#pragma once
#include <assert.h>
#include <time.h>
#include <stdlib.h>
#include <mathlib/math/test/test.hpp>
#include "block/Block.hpp"
#include "block/BlockParam.hpp"
namespace control
{
int __EXPORT basicBlocksTest();
/**
* A limiter/ saturation.
* The output of update is the input, bounded
* by min/max.
*/
class __EXPORT BlockLimit : public Block
{
public:
// methods
BlockLimit(SuperBlock *parent, const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX")
{};
virtual ~BlockLimit() {};
float update(float input);
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
protected:
// attributes
BlockParam<float> _min;
BlockParam<float> _max;
};
int __EXPORT blockLimitTest();
/**
* A symmetric limiter/ saturation.
* Same as limiter but with only a max, is used for
* upper limit of +max, and lower limit of -max
*/
class __EXPORT BlockLimitSym : public Block
{
public:
// methods
BlockLimitSym(SuperBlock *parent, const char *name) :
Block(parent, name),
_max(this, "MAX")
{};
virtual ~BlockLimitSym() {};
float update(float input);
// accessors
float getMax() { return _max.get(); }
protected:
// attributes
BlockParam<float> _max;
};
int __EXPORT blockLimitSymTest();
/**
* A low pass filter as described here:
* http://en.wikipedia.org/wiki/Low-pass_filter.
*/
class __EXPORT BlockLowPass : public Block
{
public:
// methods
BlockLowPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_state(0),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockLowPass() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCut() { return _fCut.get(); }
void setState(float state) { _state = state; }
protected:
// attributes
float _state;
BlockParam<float> _fCut;
};
int __EXPORT blockLowPassTest();
/**
* A high pass filter as described here:
* http://en.wikipedia.org/wiki/High-pass_filter.
*/
class __EXPORT BlockHighPass : public Block
{
public:
// methods
BlockHighPass(SuperBlock *parent, const char *name) :
Block(parent, name),
_u(0),
_y(0),
_fCut(this, "") // only one parameter, no need to name
{};
virtual ~BlockHighPass() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getFCut() {return _fCut.get();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
BlockParam<float> _fCut; /**< cut-off frequency, Hz */
};
int __EXPORT blockHighPassTest();
/**
* A rectangular integrator.
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegral: public SuperBlock
{
public:
// methods
BlockIntegral(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegral() {};
float update(float input);
// accessors
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setY(float y) {_y = y;}
protected:
// attributes
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTest();
/**
* A trapezoidal integrator.
* http://en.wikipedia.org/wiki/Trapezoidal_rule
* A limiter is built into the class to bound the
* integral's internal state. This is important
* for windup protection.
* @see Limit
*/
class __EXPORT BlockIntegralTrap : public SuperBlock
{
public:
// methods
BlockIntegralTrap(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_y(0),
_limit(this, "") {};
virtual ~BlockIntegralTrap() {};
float update(float input);
// accessors
float getU() {return _u;}
float getY() {return _y;}
float getMax() {return _limit.getMax();}
void setU(float u) {_u = u;}
void setY(float y) {_y = y;}
protected:
// attributes
float _u; /**< previous input */
float _y; /**< previous output */
BlockLimitSym _limit; /**< limiter */
};
int __EXPORT blockIntegralTrapTest();
/**
* A simple derivative approximation.
* This uses the previous and current input.
* This has a built in low pass filter.
* @see LowPass
*/
class __EXPORT BlockDerivative : public SuperBlock
{
public:
// methods
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
float update(float input);
// accessors
void setU(float u) {_u = u;}
float getU() {return _u;}
float getLP() {return _lowPass.getFCut();}
protected:
// attributes
float _u; /**< previous input */
BlockLowPass _lowPass; /**< low pass filter */
};
int __EXPORT blockDerivativeTest();
/**
* A proportional controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockP: public Block
{
public:
// methods
BlockP(SuperBlock *parent, const char *name) :
Block(parent, name),
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input) {
return getKP() * input;
}
// accessors
float getKP() { return _kP.get(); }
protected:
BlockParam<float> _kP;
};
int __EXPORT blockPTest();
/**
* A proportional-integral controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPI: public SuperBlock
{
public:
// methods
BlockPI(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_kP(this, "P"),
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input) {
return getKP() * input +
getKI() * getIntegral().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
BlockIntegral &getIntegral() { return _integral; }
private:
BlockIntegral _integral;
BlockParam<float> _kP;
BlockParam<float> _kI;
};
int __EXPORT blockPITest();
/**
* A proportional-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPD: public SuperBlock
{
public:
// methods
BlockPD(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_derivative(this, "D"),
_kP(this, "P"),
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input) {
return getKP() * input +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKD() { return _kD.get(); }
BlockDerivative &getDerivative() { return _derivative; }
private:
BlockDerivative _derivative;
BlockParam<float> _kP;
BlockParam<float> _kD;
};
int __EXPORT blockPDTest();
/**
* A proportional-integral-derivative controller.
* @link http://en.wikipedia.org/wiki/PID_controller
*/
class __EXPORT BlockPID: public SuperBlock
{
public:
// methods
BlockPID(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_integral(this, "I"),
_derivative(this, "D"),
_kP(this, "P"),
_kI(this, "I"),
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input) {
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
}
// accessors
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
float getKD() { return _kD.get(); }
BlockIntegral &getIntegral() { return _integral; }
BlockDerivative &getDerivative() { return _derivative; }
private:
// attributes
BlockIntegral _integral;
BlockDerivative _derivative;
BlockParam<float> _kP;
BlockParam<float> _kI;
BlockParam<float> _kD;
};
int __EXPORT blockPIDTest();
/**
* An output trim/ saturation block
*/
class __EXPORT BlockOutput: public SuperBlock
{
public:
// methods
BlockOutput(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0) {
update(0);
};
virtual ~BlockOutput() {};
void update(float input) {
_val = _limit.update(input + getTrim());
}
// accessors
float getMin() { return _limit.getMin(); }
float getMax() { return _limit.getMax(); }
float getTrim() { return _trim.get(); }
float get() { return _val; }
private:
// attributes
BlockParam<float> _trim;
BlockLimit _limit;
float _val;
};
int __EXPORT blockOutputTest();
/**
* A uniform random number generator
*/
class __EXPORT BlockRandUniform: public Block
{
public:
// methods
BlockRandUniform(SuperBlock *parent,
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX") {
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update() {
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
return getMin() + (rand_val * bounds) / rand_max;
}
// accessors
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
private:
// attributes
BlockParam<float> _min;
BlockParam<float> _max;
};
int __EXPORT blockRandUniformTest();
class __EXPORT BlockRandGauss: public Block
{
public:
// methods
BlockRandGauss(SuperBlock *parent,
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV") {
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update() {
static float V1, V2, S;
static int phase = 0;
float X;
if (phase == 0) {
do {
float U1 = (float)rand() / MAX_RAND;
float U2 = (float)rand() / MAX_RAND;
V1 = 2 * U1 - 1;
V2 = 2 * U2 - 1;
S = V1 * V1 + V2 * V2;
} while (S >= 1 || fabsf(S) < 1e-8f);
X = V1 * float(sqrt(-2 * float(log(S)) / S));
} else
X = V2 * float(sqrt(-2 * float(log(S)) / S));
phase = 1 - phase;
return X * getStdDev() + getMean();
}
// accessors
float getMean() { return _mean.get(); }
float getStdDev() { return _stdDev.get(); }
private:
// attributes
BlockParam<float> _mean;
BlockParam<float> _stdDev;
};
int __EXPORT blockRandGaussTest();
} // namespace control

367
apps/controllib/fixedwing.cpp Normal file
View File

@ -0,0 +1,367 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing.cpp
*
* Controller library code
*/
#include "fixedwing.hpp"
namespace control
{
namespace fixedwing
{
BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_rLowPass(this, "R_LP"),
_rWashout(this, "R_HP"),
_r2Rdr(this, "R2RDR"),
_rudder(0)
{
}
BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r)
{
_rudder = _r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_yawDamper(this, ""),
_pLowPass(this, "P_LP"),
_qLowPass(this, "Q_LP"),
_p2Ail(this, "P2AIL"),
_q2Elv(this, "Q2ELV"),
_aileron(0),
_elevator(0)
{
}
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r)
{
_aileron = _p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = _q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r);
}
BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_psi2Phi(this, "PSI2PHI"),
_phi2P(this, "PHI2P"),
_phiLimit(this, "PHI_LIM")
{
}
BlockHeadingHold::~BlockHeadingHold() {};
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
{
float psiError = _wrap_pi(psiCmd - psi);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
return _phi2P.update(phiCmd - phi);
}
BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Theta(this, "V2THE"),
_theta2Q(this, "THE2Q"),
_theLimit(this, "THE"),
_vLimit(this, "V")
{
}
BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
{
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
return _theta2Q.update(thetaCmd - theta);
}
BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_v2Thr(this, "V2THR")
{
}
BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
float BlockVelocityHoldFrontside::update(float vCmd, float v)
{
return _v2Thr.update(vCmd - v);
}
BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Thr(this, "H2THR"),
_throttle(0)
{
}
BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
void BlockAltitudeHoldBackside::update(float hCmd, float h)
{
_throttle = _h2Thr.update(hCmd - h);
}
BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_h2Theta(this, "H2THE"),
_theta2Q(this, "THE2Q")
{
}
BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
{
float thetaCmd = _h2Theta.update(hCmd - h);
return _theta2Q.update(thetaCmd - theta);
}
BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
const char *name,
BlockStabilization *stabilization) :
SuperBlock(parent, name),
_stabilization(stabilization),
_headingHold(this, ""),
_velocityHold(this, ""),
_altitudeHold(this, ""),
_trimAil(this, "TRIM_AIL"),
_trimElv(this, "TRIM_ELV"),
_trimRdr(this, "TRIM_RDR"),
_trimThr(this, "TRIM_THR")
{
}
BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
float h, float v,
float phi, float theta, float psi,
float p, float q, float r)
{
_altitudeHold.update(hCmd, h);
_stabilization->update(
_headingHold.update(psiCmd, phi, psi, p),
_velocityHold.update(vCmd, v, theta, q),
rCmd,
p, q, r);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
_xt2Yaw(this, "XT2YAW"),
_psiCmd(0)
{
}
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd)
{
// heading to waypoint
float psiTrack = get_bearing_to_next_waypoint(
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
// cross track
struct crosstrack_error_s xtrackError;
get_distance_to_line(&xtrackError,
(double)pos.lat / (double)1e7d,
(double)pos.lon / (double)1e7d,
(double)lastPosCmd.lat / (double)1e7d,
(double)lastPosCmd.lon / (double)1e7d,
(double)posCmd.lat / (double)1e7d,
(double)posCmd.lon / (double)1e7d);
_psiCmd = _wrap_2pi(psiTrack -
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
BlockUorbEnabledAutopilot(parent, name),
_stabilization(this, ""), // no name needed, already unique
_backsideAutopilot(this, "", &_stabilization),
_guide(this, ""),
_vCmd(this, "V_CMD"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
{
_attPoll.fd = _att.getHandle();
_attPoll.events = POLLIN;
}
void BlockMultiModeBacksideAutopilot::update()
{
// wait for a sensor update, check for exit condition every 100 ms
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
uint64_t newTimeStamp = hrt_absolute_time();
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
_timeStamp = newTimeStamp;
// check for sane values of dt
// to prevent large control responses
if (dt > 1.0f || dt < 0) return;
// set dt for all child blocks
setDt(dt);
// store old position command before update if new command sent
if (_posCmd.updated()) {
_lastPosCmd = _posCmd.getData();
}
// check for new updates
if (_param_update.updated()) updateParams();
// get new information from subscriptions
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
}
// XXX handle STABILIZED (loiter on spot) as well
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// commands
float rCmd = 0;
_backsideAutopilot.update(
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
_pos.alt, v,
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
_stabilization.update(
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
}
// update all publications
updatePublications();
}
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
updatePublications();
}
} // namespace fixedwing
} // namespace control

438
apps/controllib/fixedwing.hpp Normal file
View File

@ -0,0 +1,438 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file fixedwing.h
*
* Controller library code
*/
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include "blocks.hpp"
#include "block/UOrbSubscription.hpp"
#include "block/UOrbPublication.hpp"
extern "C" {
#include <systemlib/geo/geo.h>
}
namespace control
{
namespace fixedwing
{
/**
* BlockYawDamper
*
* This block has more explations to help new developers
* add their own blocks. It includes a limited explanation
* of some C++ basics.
*
* Block: The generic class describing a typical block as you
* would expect in Simulink or ScicosLab. A block can have
* parameters. It cannot have other blocks.
*
* SuperBlock: A superblock is a block that can have other
* blocks. It has methods that manage the blocks beneath it.
*
* BlockYawDamper inherits from SuperBlock publically, this
* means that any public function in SuperBlock are public within
* BlockYawDamper and may be called from outside the
* class methods. Any protected function within block
* are private to the class and may not be called from
* outside this class. Protected should be preferred
* where possible to public as it is important to
* limit access to the bare minimum to prevent
* accidental errors.
*/
class __EXPORT BlockYawDamper : public SuperBlock
{
private:
/**
* Declaring other blocks used by this block
*
* In this section we declare all child blocks that
* this block is composed of. They are private
* members so only this block has direct access to
* them.
*/
BlockLowPass _rLowPass;
BlockHighPass _rWashout;
BlockP _r2Rdr;
/**
* Declaring output values and accessors
*
* If we have any output values for the block we
* declare them here. Output can be directly returned
* through the update function, but outputs should be
* declared here if the information will likely be requested
* again, or if there are multiple outputs.
*
* You should only be able to set outputs from this block,
* so the outputs are declared in the private section.
* A get accessor is provided
* in the public section for other blocks to get the
* value of the output.
*/
float _rudder;
public:
/**
* BlockYawDamper Constructor
*
* The job of the constructor is to initialize all
* parameter in this block and initialize all child
* blocks. Note also, that the output values must be
* initialized here. The order of the members in the
* member initialization list should follow the
* order in which they are declared within the class.
* See the private member declarations above.
*
* Block Construction
*
* All blocks are constructed with their parent block
* and their name. This allows parameters within the
* block to construct a fully qualified name from
* concatenating the two. If the name provided to the
* block is "", then the block will use the parent
* name as it's name. This is useful in cases where
* you have a block that has parameters "MIN", "MAX",
* such as BlockLimit and you do not want an extra name
* to qualify them since the parent block has no "MIN",
* "MAX" parameters.
*
* Block Parameter Construction
*
* Block parameters are named constants, they are
* constructed using:
* BlockParam::BlockParam(Block * parent, const char * name)
* This funciton takes both a parent block and a name.
* The constructore then uses the parent name and the name of
* the paramter to ask the px4 param library if it has any
* parameters with this name. If it does, a handle to the
* parameter is retrieved.
*
* Block/ BlockParam Naming
*
* When desigining new blocks, the naming of the parameters and
* blocks determines the fully qualified name of the parameters
* within the ground station, so it is important to choose
* short, easily understandable names. Again, when a name of
* "" is passed, the parent block name is used as the value to
* prepend to paramters names.
*/
BlockYawDamper(SuperBlock *parent, const char *name);
/**
* Block deconstructor
*
* It is always a good idea to declare a virtual
* deconstructor so that upon calling delete from
* a class derived from this, all of the
* deconstructors all called, the derived class first, and
* then the base class
*/
virtual ~BlockYawDamper();
/**
* Block update function
*
* The job of the update function is to compute the output
* values for the block. In a simple block with one output,
* the output may be returned directly. If the output is
* required frequenly by other processses, it might be a
* good idea to declare a member to store the temporary
* variable.
*/
void update(float rCmd, float r);
/**
* Rudder output value accessor
*
* This is a public accessor function, which means that the
* private value _rudder is returned to anyone calling
* BlockYawDamper::getRudder(). Note thate a setRudder() is
* not provided, this is because the updateParams() call
* for a block provides the mechanism for updating the
* paramter.
*/
float getRudder() { return _rudder; }
};
/**
* Stability augmentation system.
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
*/
class __EXPORT BlockStabilization : public SuperBlock
{
private:
BlockYawDamper _yawDamper;
BlockLowPass _pLowPass;
BlockLowPass _qLowPass;
BlockP _p2Ail;
BlockP _q2Elv;
float _aileron;
float _elevator;
public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
float p, float q, float r);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
};
/**
* Heading hold autopilot block.
* Aircraft Control and Simulation, Stevens and Lewis
* Heading hold, pg. 348
*/
class __EXPORT BlockHeadingHold : public SuperBlock
{
private:
BlockP _psi2Phi;
BlockP _phi2P;
BlockLimitSym _phiLimit;
public:
BlockHeadingHold(SuperBlock *parent, const char *name);
virtual ~BlockHeadingHold();
/**
* returns pCmd
*/
float update(float psiCmd, float phi, float psi, float p);
};
/**
* Frontside/ Backside Control Systems
*
* Frontside :
* velocity error -> throttle
* altitude error -> elevator
*
* Backside :
* velocity error -> elevator
* altitude error -> throttle
*
* Backside control systems are more resilient at
* slow speeds on the back-side of the power
* required curve/ landing etc. Less performance
* than frontside at high speeds.
*/
/**
* Backside velocity hold autopilot block.
* v -> theta -> q -> elevator
*/
class __EXPORT BlockVelocityHoldBackside : public SuperBlock
{
private:
BlockPID _v2Theta;
BlockPID _theta2Q;
BlockLimit _theLimit;
BlockLimit _vLimit;
public:
BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
virtual ~BlockVelocityHoldBackside();
/**
* returns qCmd
*/
float update(float vCmd, float v, float theta, float q);
};
/**
* Frontside velocity hold autopilot block.
* v -> throttle
*/
class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
{
private:
BlockPID _v2Thr;
public:
BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
virtual ~BlockVelocityHoldFrontside();
/**
* returns throttle
*/
float update(float vCmd, float v);
};
/**
* Backside altitude hold autopilot block.
* h -> throttle
*/
class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
{
private:
BlockPID _h2Thr;
float _throttle;
public:
BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
virtual ~BlockAltitudeHoldBackside();
void update(float hCmd, float h);
float getThrottle() { return _throttle; }
};
/**
* Frontside altitude hold autopilot block.
* h -> theta > q -> elevator
*/
class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
{
private:
BlockPID _h2Theta;
BlockPID _theta2Q;
public:
BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
virtual ~BlockAltitudeHoldFrontside();
/**
* return qCmd
*/
float update(float hCmd, float h, float theta, float q);
};
/**
* Backside autopilot
*/
class __EXPORT BlockBacksideAutopilot : public SuperBlock
{
private:
BlockStabilization *_stabilization;
BlockHeadingHold _headingHold;
BlockVelocityHoldBackside _velocityHold;
BlockAltitudeHoldBackside _altitudeHold;
BlockParam<float> _trimAil;
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
public:
BlockBacksideAutopilot(SuperBlock *parent,
const char *name,
BlockStabilization *stabilization);
virtual ~BlockBacksideAutopilot();
void update(float hCmd, float vCmd, float rCmd, float psiCmd,
float h, float v,
float phi, float theta, float psi,
float p, float q, float r);
float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
};
/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
BlockLimitSym _xtYawLimit;
BlockP _xt2Yaw;
float _psiCmd;
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
vehicle_global_position_setpoint_s &posCmd,
vehicle_global_position_setpoint_s &lastPosCmd);
float getPsiCmd() { return _psiCmd; }
};
/**
* UorbEnabledAutopilot
*/
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
UOrbSubscription<vehicle_attitude_s> _att;
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
UOrbSubscription<vehicle_global_position_s> _pos;
UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
UOrbSubscription<manual_control_setpoint_s> _manual;
UOrbSubscription<vehicle_status_s> _status;
UOrbSubscription<parameter_update_s> _param_update;
// publications
UOrbPublication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
};
/**
* Multi-mode Autopilot
*/
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
{
private:
BlockStabilization _stabilization;
BlockBacksideAutopilot _backsideAutopilot;
BlockWaypointGuidance _guide;
BlockParam<float> _vCmd;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
uint64_t _timeStamp;
public:
BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
void update();
virtual ~BlockMultiModeBacksideAutopilot();
};
} // namespace fixedwing
} // namespace control

22
apps/controllib/test_params.c Normal file
View File

@ -0,0 +1,22 @@
#include <systemlib/param/param.h>
// WARNING:
// do not changes these unless
// you want to recompute the
// answers for all of the unit tests
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);

449
apps/drivers/blinkm/blinkm.cpp
View File

@ -35,9 +35,61 @@
* @file blinkm.cpp
*
* Driver for the BlinkM LED controller connected via I2C.
*
* Connect the BlinkM to I2C3 and put the following line to the rc startup-script:
* blinkm start
*
* To start the system monitor put in the next line after the blinkm start:
* blinkm systemmonitor
*
*
* Description:
* After startup, the Application checked how many lipo cells are connected to the System.
* The recognized number off cells, will be blinked 5 times in purple color.
* 2 Cells = 2 blinks
* ...
* 5 Cells = 5 blinks
* Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
*
* System disarmed:
* The BlinkM should lit solid red.
*
* System armed:
* One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
*
* X-X-X-X-_-_-_-_-
* -------------------------
* G G G M
* P P P O
* S S S D
* E
*
* (X = on, _=off)
*
* The first 3 blinks indicates the status of the GPS-Signal (red):
* 0-4 satellites = X-X-X-X-_-_-_-_-
* 5 satellites = X-X-_-X-_-_-_-_-
* 6 satellites = X-_-_-X-_-_-_-_-
* >=7 satellites = _-_-_-X-_-_-_-_-
* If no GPS is found the first 3 blinks are white
*
* The fourth Blink indicates the Flightmode:
* MANUAL : off
* STABILIZED : yellow
* HOLD : blue
* AUTO : green
*
* Battery Warning (low Battery Level):
* Continuously blinking in yellow X-X-X-X-X-X-X-X
*
* Battery Alert (critical Battery Level)
* Continuously blinking in red X-X-X-X-X-X-X-X
*
* General Error (no uOrb Data)
* Continuously blinking in white X-X-X-X-X-X-X-X
*
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@ -59,15 +111,28 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <poll.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
static const int LED_ONTIME = 100;
static const int LED_OFFTIME = 100;
static const int LED_BLINK = 1;
static const int LED_NOBLINK = 0;
class BlinkM : public device::I2C
{
public:
BlinkM(int bus);
~BlinkM();
virtual int init();
virtual int probe();
virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
static const char *script_names[];
@ -95,11 +160,31 @@ private:
MORSE_CODE
};
work_s _work;
static const unsigned _monitor_interval = 250;
enum ledColors {
LED_OFF,
LED_RED,
LED_YELLOW,
LED_PURPLE,
LED_GREEN,
LED_BLUE,
LED_WHITE
};
static void monitor_trampoline(void *arg);
void monitor();
work_s _work;
int led_color_1;
int led_color_2;
int led_color_3;
int led_color_4;
int led_color_5;
int led_color_6;
int led_blink;
bool systemstate_run;
void setLEDColor(int ledcolor);
static void led_trampoline(void *arg);
void led();
int set_rgb(uint8_t r, uint8_t g, uint8_t b);
@ -127,8 +212,7 @@ private:
/* for now, we only support one BlinkM */
namespace
{
BlinkM *g_blinkm;
BlinkM *g_blinkm;
}
/* list of script names, must match script ID numbers */
@ -155,11 +239,21 @@ const char *BlinkM::script_names[] = {
nullptr
};
extern "C" __EXPORT int blinkm_main(int argc, char *argv[]);
BlinkM::BlinkM(int bus) :
I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000)
I2C("blinkm", BLINKM_DEVICE_PATH, bus, 0x09, 100000),
led_color_1(LED_OFF),
led_color_2(LED_OFF),
led_color_3(LED_OFF),
led_color_4(LED_OFF),
led_color_5(LED_OFF),
led_color_6(LED_OFF),
led_blink(LED_NOBLINK),
systemstate_run(false)
{
memset(&_work, 0, sizeof(_work));
}
BlinkM::~BlinkM()
@ -170,7 +264,6 @@ int
BlinkM::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
@ -178,14 +271,25 @@ BlinkM::init()
return ret;
}
/* set some sensible defaults */
set_fade_speed(25);
stop_script();
set_rgb(0,0,0);
/* turn off by default */
play_script(BLACK);
return OK;
}
/* start the system monitor as a low-priority workqueue entry */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, 1);
int
BlinkM::setMode(int mode)
{
if(mode == 1) {
if(systemstate_run == false) {
stop_script();
set_rgb(0,0,0);
systemstate_run = true;
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, 1);
}
} else {
systemstate_run = false;
}
return OK;
}
@ -249,21 +353,300 @@ BlinkM::ioctl(struct file *filp, int cmd, unsigned long arg)
return ret;
}
void
BlinkM::monitor_trampoline(void *arg)
BlinkM::led_trampoline(void *arg)
{
BlinkM *bm = (BlinkM *)arg;
bm->monitor();
bm->led();
}
void
BlinkM::monitor()
{
/* check system state, possibly update LED to suit */
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::monitor_trampoline, this, _monitor_interval);
void
BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_gps_position_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
static int t_led_color[6] = { 0, 0, 0, 0, 0, 0};
static int t_led_blink = 0;
static int led_thread_runcount=0;
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
static bool detected_cells_blinked = false;
static bool led_thread_ready = true;
int num_of_used_sats = 0;
if(!topic_initialized) {
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
topic_initialized = true;
}
if(led_thread_ready == true) {
if(!detected_cells_blinked) {
if(num_of_cells > 0) {
t_led_color[0] = LED_PURPLE;
}
if(num_of_cells > 1) {
t_led_color[1] = LED_PURPLE;
}
if(num_of_cells > 2) {
t_led_color[2] = LED_PURPLE;
}
if(num_of_cells > 3) {
t_led_color[3] = LED_PURPLE;
}
if(num_of_cells > 4) {
t_led_color[4] = LED_PURPLE;
}
t_led_color[5] = LED_OFF;
t_led_blink = LED_BLINK;
} else {
t_led_color[0] = led_color_1;
t_led_color[1] = led_color_2;
t_led_color[2] = led_color_3;
t_led_color[3] = led_color_4;
t_led_color[4] = led_color_5;
t_led_color[5] = led_color_6;
t_led_blink = led_blink;
}
led_thread_ready = false;
}
if (led_thread_runcount & 1) {
if (t_led_blink)
setLEDColor(LED_OFF);
led_interval = LED_OFFTIME;
} else {
setLEDColor(t_led_color[(led_thread_runcount / 2) % 6]);
//led_interval = (led_thread_runcount & 1) : LED_ONTIME;
led_interval = LED_ONTIME;
}
if (led_thread_runcount == 11) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_gps_position_s vehicle_gps_position_raw;
bool new_data_vehicle_status;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
if (new_data_vehicle_status) {
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
no_data_vehicle_status = 0;
} else {
no_data_vehicle_status++;
if(no_data_vehicle_status >= 3)
no_data_vehicle_status = 3;
}
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub_fd, &vehicle_gps_position_raw);
no_data_vehicle_gps_position = 0;
} else {
no_data_vehicle_gps_position++;
if(no_data_vehicle_gps_position >= 3)
no_data_vehicle_gps_position = 3;
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
for(int satloop=0; satloop<20; satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
if(new_data_vehicle_status || no_data_vehicle_status < 3){
if(num_of_cells == 0) {
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
/* LED Pattern for battery low warning */
led_color_1 = LED_YELLOW;
led_color_2 = LED_YELLOW;
led_color_3 = LED_YELLOW;
led_color_4 = LED_YELLOW;
led_color_5 = LED_YELLOW;
led_color_6 = LED_YELLOW;
led_blink = LED_BLINK;
} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
/* LED Pattern for battery critical alerting */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_blink = LED_BLINK;
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_system_armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_RED;
led_color_5 = LED_RED;
led_color_6 = LED_RED;
led_blink = LED_NOBLINK;
} else {
/* armed system - initial led pattern */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
led_color_3 = LED_RED;
led_color_4 = LED_OFF;
led_color_5 = LED_OFF;
led_color_6 = LED_OFF;
led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
led_color_4 = LED_OFF;
break;
case VEHICLE_FLIGHT_MODE_STAB:
led_color_4 = LED_YELLOW;
break;
case VEHICLE_FLIGHT_MODE_HOLD:
led_color_4 = LED_BLUE;
break;
case VEHICLE_FLIGHT_MODE_AUTO:
led_color_4 = LED_GREEN;
break;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat´s */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 6) {
led_color_2 = LED_OFF;
led_color_3 = LED_OFF;
} else if(num_of_used_sats == 5) {
led_color_3 = LED_OFF;
}
} else {
/* no vehicle_gps_position data */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
}
}
}
}
} else {
/* LED Pattern for general Error - no vehicle_status can retrieved */
led_color_1 = LED_WHITE;
led_color_2 = LED_WHITE;
led_color_3 = LED_WHITE;
led_color_4 = LED_WHITE;
led_color_5 = LED_WHITE;
led_color_6 = LED_WHITE;
led_blink = LED_BLINK;
}
/*
printf( "<blinkm> Volt:%8.4f\tArmed:%4u\tMode:%4u\tCells:%4u\tNDVS:%4u\tNDSAT:%4u\tSats:%4u\tFix:%4u\tVisible:%4u\n",
vehicle_status_raw.voltage_battery,
vehicle_status_raw.flag_system_armed,
vehicle_status_raw.flight_mode,
num_of_cells,
no_data_vehicle_status,
no_data_vehicle_gps_position,
num_of_used_sats,
vehicle_gps_position_raw.fix_type,
vehicle_gps_position_raw.satellites_visible);
*/
led_thread_runcount=0;
led_thread_ready = true;
led_interval = LED_OFFTIME;
if(detected_cells_runcount < 4){
detected_cells_runcount++;
} else {
detected_cells_blinked = true;
}
} else {
led_thread_runcount++;
}
if(systemstate_run == true) {
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&BlinkM::led_trampoline, this, led_interval);
} else {
stop_script();
set_rgb(0,0,0);
}
}
void BlinkM::setLEDColor(int ledcolor) {
switch (ledcolor) {
case LED_OFF: // off
set_rgb(0,0,0);
break;
case LED_RED: // red
set_rgb(255,0,0);
break;
case LED_YELLOW: // yellow
set_rgb(255,70,0);
break;
case LED_PURPLE: // purple
set_rgb(255,0,255);
break;
case LED_GREEN: // green
set_rgb(0,255,0);
break;
case LED_BLUE: // blue
set_rgb(0,0,255);
break;
case LED_WHITE: // white
set_rgb(255,255,255);
break;
}
}
int
@ -413,7 +796,6 @@ BlinkM::get_rgb(uint8_t &r, uint8_t &g, uint8_t &b)
return ret;
}
int
BlinkM::get_firmware_version(uint8_t version[2])
{
@ -443,9 +825,21 @@ blinkm_main(int argc, char *argv[])
exit(0);
}
if (g_blinkm == nullptr)
errx(1, "not started");
if (!strcmp(argv[1], "systemstate")) {
g_blinkm->setMode(1);
exit(0);
}
if (!strcmp(argv[1], "ledoff")) {
g_blinkm->setMode(0);
exit(0);
}
if (!strcmp(argv[1], "list")) {
for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++)
fprintf(stderr, " %s\n", BlinkM::script_names[i]);
@ -458,8 +852,9 @@ blinkm_main(int argc, char *argv[])
if (fd < 0)
err(1, "can't open BlinkM device");
g_blinkm->setMode(0);
if (ioctl(fd, BLINKM_PLAY_SCRIPT_NAMED, (unsigned long)argv[1]) == OK)
exit(0);
errx(1, "missing command, try 'start', 'list' or a script name");
}
errx(1, "missing command, try 'start', 'systemstate', 'ledoff', 'list' or a script name.");
}

59
apps/drivers/boards/px4fmu/px4fmu_init.c
View File

@ -95,8 +95,6 @@
* Protected Functions
****************************************************************************/
extern int adc_devinit(void);
/****************************************************************************
* Public Functions
****************************************************************************/
@ -150,9 +148,7 @@ __EXPORT int nsh_archinitialize(void)
int result;
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
@ -160,27 +156,21 @@ __EXPORT int nsh_archinitialize(void)
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
// initial LED state
drv_led_start();
@ -209,8 +199,7 @@ __EXPORT int nsh_archinitialize(void)
message("[boot] Successfully initialized SPI port 1\r\n");
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
@ -233,23 +222,11 @@ __EXPORT int nsh_archinitialize(void)
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
#endif /* SPI3 */
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK) {
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK) {
/* Give up */
message("[boot] FAILED adc_devinit: %d\n", adc_state);
return -ENODEV;
}
}
#endif
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
stm32_configgpio(GPIO_ADC1_IN12);
stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards
return OK;
}

3
apps/drivers/boards/px4io/px4io_init.c
View File

@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ACC_OC_DETECT);
stm32_configgpio(GPIO_SERVO_OC_DETECT);
stm32_configgpio(GPIO_BTN_SAFETY);
stm32_configgpio(GPIO_ADC_VBATT);
stm32_configgpio(GPIO_ADC_IN5);
}

27
apps/drivers/boards/px4io/px4io_internal.h
View File

@ -61,28 +61,6 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* R/C in/out channels **************************************************************/
/* XXX just GPIOs for now - eventually timer pins */
#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
/* Safety switch button *************************************************************/
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@ -98,3 +76,8 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
/* Analog inputs ********************************************************************/
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
#define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)

52
apps/drivers/drv_adc.h Normal file
View File

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

23
apps/drivers/drv_mixer.h
View File

@ -100,28 +100,13 @@ struct mixer_simple_s {
*/
#define MIXERIOCADDSIMPLE _MIXERIOC(2)
/** multirotor output definition */
struct mixer_rotor_output_s {
float angle; /**< rotor angle clockwise from forward in radians */
float distance; /**< motor distance from centre in arbitrary units */
};
/** multirotor mixer */
struct mixer_multirotor_s {
uint8_t rotor_count;
struct mixer_control_s controls[4]; /**< controls are roll, pitch, yaw, thrust */
struct mixer_rotor_output_s rotors[0]; /**< actual size of the array is set by rotor_count */
};
/* _MIXERIOC(3) was deprecated */
/* _MIXERIOC(4) was deprecated */
/**
* Add a multirotor mixer in (struct mixer_multirotor_s *)arg
* Add mixer(s) from the buffer in (const char *)arg
*/
#define MIXERIOCADDMULTIROTOR _MIXERIOC(3)
/**
* Add mixers(s) from a the file in (const char *)arg
*/
#define MIXERIOCLOADFILE _MIXERIOC(4)
#define MIXERIOCLOADBUF _MIXERIOC(5)
/*
* XXX Thoughts for additional operations:

3
apps/drivers/drv_pwm_output.h
View File

@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

58
apps/drivers/hil/hil.cpp
View File

@ -68,11 +68,11 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
// #include <drivers/boards/HIL/HIL_internal.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@ -382,7 +382,6 @@ HIL::task_main()
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
@ -396,16 +395,27 @@ HIL::task_main()
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* output to the servo */
// up_pwm_servo_set(i, outputs.output[i]);
/* last resort: catch NaN, INF and out-of-band errors */
if (i < (unsigned)outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
}
/* and publish for anyone that cares to see */
@ -419,9 +429,6 @@ HIL::task_main()
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
/* update PWM servo armed status */
// up_pwm_servo_arm(aa.armed);
}
}
@ -503,6 +510,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
// up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -577,26 +588,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -605,10 +609,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}
default:
ret = -ENOTTY;
break;

77
apps/drivers/px4fmu/fmu.cpp
View File

@ -58,6 +58,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/boards/px4fmu/px4fmu_internal.h>
#include <drivers/drv_hrt.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
@ -65,6 +66,7 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
@ -97,6 +99,7 @@ private:
int _t_actuators;
int _t_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
bool _primary_pwm_device;
@ -162,6 +165,7 @@ PX4FMU::PX4FMU() :
_t_actuators(-1),
_t_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
@ -319,6 +323,13 @@ PX4FMU::task_main()
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
/* advertise the effective control inputs */
actuator_controls_effective_s controls_effective;
memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective);
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
@ -336,8 +347,16 @@ PX4FMU::task_main()
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
if (update_rate_in_ms < 2)
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
_update_rate = 50;
}
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
@ -364,20 +383,39 @@ PX4FMU::task_main()
if (_mixers != nullptr) {
/* do mixing */
_mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
}
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
@ -394,6 +432,7 @@ PX4FMU::task_main()
}
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
/* make sure servos are off */
@ -470,6 +509,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -500,7 +543,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
channel = cmd - PWM_SERVO_SET(0);
channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
@ -544,28 +587,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
case MIXERIOCLOADFILE: {
const char *path = (const char *)arg;
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (_mixers == nullptr) {
ret = -ENOMEM;
} else {
debug("loading mixers from %s", path);
ret = _mixers->load_from_file(path);
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
debug("mixer load failed with %d", ret);
@ -574,7 +608,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
}
}
break;
}

420
apps/drivers/px4io/px4io.cpp
View File

@ -55,12 +55,14 @@
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <math.h>
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@ -69,10 +71,15 @@
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <px4io/protocol.h>
#include "uploader.h"
@ -88,10 +95,18 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
/**
* Set the PWM via serial update rate
* @warning this directly affects CPU load
*/
int set_pwm_rate(int hz);
bool dump_one;
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
// XXX
static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
unsigned _update_rate; ///< serial send rate in Hz
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
@ -103,19 +118,30 @@ private:
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
actuator_controls_effective_s _controls_effective; ///< effective controls
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
int _t_vstatus; ///< system / vehicle status
vehicle_status_s _vstatus; ///< overall system state
orb_advert_t _to_input_rc; ///< rc inputs from io
rc_input_values _input_rc; ///< rc input values
orb_advert_t _to_battery; ///< battery status / voltage
battery_status_s _battery_status;///< battery status data
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
const char *volatile _mix_buf; ///< mixer text buffer
volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
bool _primary_pwm_device; ///< true if we are the default PWM output
uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
@ -157,6 +183,11 @@ private:
*/
void config_send();
/**
* Send a buffer containing mixer text to PX4IO
*/
int mixer_send(const char *buf, unsigned buflen);
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
@ -178,19 +209,24 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
dump_one(false),
_update_rate(50),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_connected(false),
_t_actuators(-1),
_t_actuators_effective(-1),
_t_armed(-1),
_t_vstatus(-1),
_t_outputs(-1),
_mixers(nullptr),
_mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false),
_relays(0),
_switch_armed(false),
_send_needed(false),
_config_needed(false)
_config_needed(true)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@ -208,6 +244,7 @@ PX4IO::~PX4IO()
/* give it another 100ms */
usleep(100000);
}
/* well, kill it anyway, though this will probably crash */
if (_task != -1)
task_delete(_task);
@ -237,7 +274,8 @@ PX4IO::init()
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
@ -249,16 +287,30 @@ PX4IO::init()
debug("PX4IO connected");
break;
}
usleep(100000);
}
if (!_connected) {
/* error here will result in everything being torn down */
log("PX4IO not responding");
return -EIO;
}
return OK;
}
int
PX4IO::set_pwm_rate(int hz)
{
if (hz > 0 && hz <= 400) {
_update_rate = hz;
return OK;
} else {
return -EINVAL;
}
}
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@ -269,6 +321,7 @@ void
PX4IO::task_main()
{
log("starting");
unsigned update_rate_in_ms;
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
@ -290,10 +343,12 @@ PX4IO::task_main()
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
if (_io_stream == nullptr) {
log("failed to allocate HX protocol stream");
goto out;
}
hx_stream_set_counters(_io_stream,
perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
perf_alloc(PC_COUNT, "PX4IO frames received"),
@ -306,12 +361,20 @@ PX4IO::task_main()
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
//int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
update_rate_in_ms = 1000 / _update_rate;
orb_set_interval(_t_actuators, update_rate_in_ms);
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
/* advertise the limited control inputs */
memset(&_controls_effective, 0, sizeof(_controls_effective));
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
&_controls_effective);
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
@ -321,22 +384,33 @@ PX4IO::task_main()
memset(&_input_rc, 0, sizeof(_input_rc));
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
/* do not advertise the battery status until its clear that a battery is connected */
memset(&_battery_status, 0, sizeof(_battery_status));
_to_battery = -1;
/* poll descriptor */
pollfd fds[3];
pollfd fds[4];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
fds[3].fd = _t_vstatus;
fds[3].events = POLLIN;
log("ready");
debug("ready");
/* lock against the ioctl handler */
lock();
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */
if (ret < 0) {
@ -353,37 +427,32 @@ PX4IO::task_main()
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
/* if we have new control data from the ORB, handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* scale controls to PWM (temporary measure) */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _controls.control[i]);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
/* and flag for update */
_send_needed = true;
}
/* if we have an arming state update, handle it */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
_send_needed = true;
}
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
_send_needed = true;
}
}
/* send a config packet to IO if required */
@ -391,14 +460,32 @@ PX4IO::task_main()
_config_needed = false;
config_send();
}
/* send a mixer update if needed */
if (_mix_buf != nullptr) {
mixer_send(_mix_buf, _mix_buf_len);
/* clear the buffer record so the ioctl handler knows we're done */
_mix_buf = nullptr;
_mix_buf_len = 0;
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
unlock();
out:
debug("exiting");
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
::close(_serial_fd);
/* clean up the alternate device node */
@ -451,25 +538,27 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
lock();
// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++)
{
for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
@ -479,6 +568,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
/* remember the latched arming switch state */
_switch_armed = rep->armed;
/* publish battery information */
/* only publish if battery has a valid minimum voltage */
if (rep->battery_mv > 3300) {
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = rep->battery_mv / 1000.0f;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* announce the battery voltage if needed, just publish else */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
_send_needed = true;
/* if monitoring, dump the received info */
@ -486,13 +592,16 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n");
}
out:
unlock();
// unlock();
return;
}
void
@ -504,18 +613,29 @@ PX4IO::io_send()
cmd.f2i_magic = F2I_MAGIC;
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
for (unsigned i = 0; i < _max_actuators; i++) {
cmd.output_control[i] = _outputs.output[i];
}
/* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
_outputs.timestamp = hrt_absolute_time();
/* XXX needs to be based off post-mix values from the IO side */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
// XXX relays
/* update relays */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
/* armed and not locked down */
/* armed and not locked down -> arming ok */
cmd.arm_ok = (_armed.armed && !_armed.lockdown);
/* indicate that full autonomous position control / vector flight mode is available */
cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
/* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
/* set desired PWM output rate */
cmd.servo_rate = _update_rate;
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
debug("send error %d", ret);
}
@ -528,11 +648,88 @@ PX4IO::config_send()
cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
int val;
/* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
param_get(param_find("RC_MAP_ROLL"), &val);
cfg.rc_map[0] = val;
param_get(param_find("RC_MAP_PITCH"), &val);
cfg.rc_map[1] = val;
param_get(param_find("RC_MAP_YAW"), &val);
cfg.rc_map[2] = val;
param_get(param_find("RC_MAP_THROTTLE"), &val);
cfg.rc_map[3] = val;
/* set the individual channel properties */
char nbuf[16];
float float_val;
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MIN", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_min[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_TRIM", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_trim[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_MAX", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_max[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_REV", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_rev[i] = float_val;
}
for (unsigned i = 0; i < 4; i++) {
sprintf(nbuf, "RC%d_DZ", i + 1);
param_get(param_find(nbuf), &float_val);
cfg.rc_dz[i] = float_val;
}
ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
if (ret)
debug("config error %d", ret);
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen)
{
uint8_t frame[HX_STREAM_MAX_FRAME];
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
msg->action = F2I_MIXER_ACTION_RESET;
do {
unsigned count = buflen;
if (count > F2I_MIXER_MAX_TEXT)
count = F2I_MIXER_MAX_TEXT;
if (count > 0) {
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
}
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
if (ret) {
log("mixer send error %d", ret);
return ret;
}
msg->action = F2I_MIXER_ACTION_APPEND;
} while (buflen > 0);
return 0;
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
@ -554,9 +751,14 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
_send_needed = true;
break;
case PWM_SERVO_SET_UPDATE_RATE:
// not supported yet
ret = -EINVAL;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
/* fake an update to the selected 'servo' channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
@ -572,68 +774,53 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case GPIO_RESET:
_relays = 0;
_send_needed = true;
break;
case GPIO_SET:
case GPIO_CLEAR:
/* make sure only valid bits are being set */
if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
ret = EINVAL;
break;
}
if (cmd == GPIO_SET) {
_relays |= arg;
} else {
_relays &= ~arg;
}
_send_needed = true;
break;
case GPIO_GET:
*(uint32_t *)arg = _relays;
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
*(unsigned *)arg = PX4IO_CONTROL_CHANNELS;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
ret = 0; /* load always resets */
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
case MIXERIOCLOADBUF:
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* set the buffer up for transfer */
_mix_buf = (const char *)arg;
_mix_buf_len = strnlen(_mix_buf, 1024);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
/* drop the lock and wait for the thread to clear the transmit */
unlock();
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
while (_mix_buf != nullptr)
usleep(1000);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
lock();
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
ret = 0;
break;
default:
@ -675,13 +862,20 @@ test(void)
close(fd);
actuator_armed_s aa;
aa.armed = true;
aa.lockdown = false;
orb_advertise(ORB_ID(actuator_armed), &aa);
exit(0);
}
void
monitor(void)
{
unsigned cancels = 4;
unsigned cancels = 3;
printf("Hit <enter> three times to exit monitor mode\n");
for (;;) {
@ -694,6 +888,7 @@ monitor(void)
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
exit(0);
}
@ -702,6 +897,7 @@ monitor(void)
g_dev->dump_one = true;
}
}
}
int
@ -723,12 +919,51 @@ px4io_main(int argc, char *argv[])
errx(1, "driver init failed");
}
/* look for the optional pwm update rate for the supported modes */
if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
if (argc > 2 + 1) {
g_dev->set_pwm_rate(atoi(argv[2 + 1]));
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
}
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (g_dev != nullptr) {
/* stop the driver */
delete g_dev;
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr)
printf("[px4io] loaded\n");
else
printf("[px4io] not loaded\n");
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
printf("[px4io] loaded, detaching first\n");
/* stop the driver */
delete g_dev;
}
PX4IO_Uploader *up;
const char *fn[3];
@ -780,8 +1015,9 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "monitor"))
monitor();
errx(1, "need a command, try 'start', 'test', 'monitor' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
}

132
apps/drivers/px4io/uploader.cpp
View File

@ -51,6 +51,50 @@
#include "uploader.h"
static const uint32_t crctab[] =
{
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
static uint32_t
crc32(const uint8_t *src, unsigned len, unsigned state)
{
for (unsigned i = 0; i < len; i++)
state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
return state;
}
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
@ -110,6 +154,17 @@ PX4IO_Uploader::upload(const char *filenames[])
}
}
ret = get_info(INFO_BL_REV, bl_rev);
if (ret == OK) {
if (bl_rev <= BL_REV) {
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
return OK;
}
}
ret = erase();
if (ret != OK) {
@ -124,7 +179,11 @@ PX4IO_Uploader::upload(const char *filenames[])
continue;
}
ret = verify();
if (bl_rev <= 2)
ret = verify_rev2();
else if(bl_rev == 3) {
ret = verify_rev3();
}
if (ret != OK) {
log("verify failed");
@ -190,6 +249,7 @@ PX4IO_Uploader::drain()
do {
ret = recv(c, 250);
if (ret == OK) {
//log("discard 0x%02x", c);
}
@ -319,7 +379,7 @@ PX4IO_Uploader::program()
}
int
PX4IO_Uploader::verify()
PX4IO_Uploader::verify_rev2()
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
@ -379,6 +439,74 @@ PX4IO_Uploader::verify()
return OK;
}
int
PX4IO_Uploader::verify_rev3()
{
int ret;
uint8_t file_buf[4];
ssize_t count;
uint32_t sum = 0;
uint32_t bytes_read = 0;
uint32_t fw_size = 0;
uint32_t crc = 0;
uint8_t fill_blank = 0xff;
log("verify...");
lseek(_fw_fd, 0, SEEK_SET);
ret = get_info(INFO_FLASH_SIZE, fw_size);
send(PROTO_EOC);
if (ret != OK) {
log("could not read firmware size");
return ret;
}
/* read through the firmware file again and calculate the checksum*/
while (true) {
lseek(_fw_fd, 0, SEEK_CUR);
count = read(_fw_fd, file_buf, sizeof(file_buf));
/* set the rest to ff */
if (count == 0) {
break;
}
/* stop if the file cannot be read */
if (count < 0)
return -errno;
/* calculate crc32 sum */
sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
bytes_read += count;
}
/* fill the rest with 0xff */
while (bytes_read < fw_size) {
sum = crc32(&fill_blank, sizeof(fill_blank), sum);
bytes_read += sizeof(fill_blank);
}
/* request CRC from IO */
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");
return ret;
}
/* compare the CRC sum from the IO with the one calculated */
if (sum != crc) {
log("CRC wrong: received: %d, expected: %d", crc, sum);
return -EINVAL;
}
return OK;
}
int
PX4IO_Uploader::reboot()
{

9
apps/drivers/px4io/uploader.h
View File

@ -64,21 +64,25 @@ private:
PROTO_CHIP_VERIFY = 0x24,
PROTO_PROG_MULTI = 0x27,
PROTO_READ_MULTI = 0x28,
PROTO_GET_CRC = 0x29,
PROTO_REBOOT = 0x30,
INFO_BL_REV = 1, /**< bootloader protocol revision */
BL_REV = 2, /**< supported bootloader protocol */
BL_REV = 3, /**< supported bootloader protocol */
INFO_BOARD_ID = 2, /**< board type */
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
};
int _io_fd;
int _fw_fd;
uint32_t bl_rev; /**< bootloader revision */
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout = 1000);
@ -91,7 +95,8 @@ private:
int get_info(int param, uint32_t &val);
int erase();
int program();
int verify();
int verify_rev2();
int verify_rev3();
int reboot();
int compare(bool &identical);
};

43
apps/drivers/stm32/adc/Makefile Normal file
View File

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

387
apps/drivers/stm32/adc/adc.cpp Normal file
View File

@ -0,0 +1,387 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file adc.cpp
*
* Driver for the STM32 ADC.
*
* This is a low-rate driver, designed for sampling things like voltages
* and so forth. It avoids the gross complexity of the NuttX ADC driver.
*/
#include <nuttx/config.h>
#include <drivers/device/device.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_adc.h>
#include <arch/stm32/chip.h>
#include <stm32_internal.h>
#include <stm32_gpio.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
/*
* Register accessors.
* For now, no reason not to just use ADC1.
*/
#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
#define rSR REG(STM32_ADC_SR_OFFSET)
#define rCR1 REG(STM32_ADC_CR1_OFFSET)
#define rCR2 REG(STM32_ADC_CR2_OFFSET)
#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
#define rHTR REG(STM32_ADC_HTR_OFFSET)
#define rLTR REG(STM32_ADC_LTR_OFFSET)
#define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
#define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
#define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
#define rJSQR REG(STM32_ADC_JSQR_OFFSET)
#define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
#define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
#define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
#define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
#define rDR REG(STM32_ADC_DR_OFFSET)
#ifdef STM32_ADC_CCR
# define rCCR REG(STM32_ADC_CCR_OFFSET)
#endif
class ADC : public device::CDev
{
public:
ADC(uint32_t channels);
~ADC();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t read(file *filp, char *buffer, size_t len);
protected:
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
private:
static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */
hrt_call _call;
perf_counter_t _sample_perf;
unsigned _channel_count;
adc_msg_s *_samples; /**< sample buffer */
/** work trampoline */
static void _tick_trampoline(void *arg);
/** worker function */
void _tick();
/**
* Sample a single channel and return the measured value.
*
* @param channel The channel to sample.
* @return The sampled value, or 0xffff if
* sampling failed.
*/
uint16_t _sample(unsigned channel);
};
ADC::ADC(uint32_t channels) :
CDev("adc", ADC_DEVICE_PATH),
_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
_channel_count(0),
_samples(nullptr)
{
_debug_enabled = true;
/* always enable the temperature sensor */
channels |= 1 << 16;
/* allocate the sample array */
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_channel_count++;
}
}
_samples = new adc_msg_s[_channel_count];
/* prefill the channel numbers in the sample array */
if (_samples != nullptr) {
unsigned index = 0;
for (unsigned i = 0; i < 32; i++) {
if (channels & (1 << i)) {
_samples[index].am_channel = i;
_samples[index].am_data = 0;
index++;
}
}
}
}
ADC::~ADC()
{
if (_samples != nullptr)
delete _samples;
}
int
ADC::init()
{
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_CAL;
usleep(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
rCR2 |= ADC_CR2_ADON;
usleep(10);
/* kick off a sample and wait for it to complete */
hrt_abstime now = hrt_absolute_time();
rCR2 |= ADC_CR2_SWSTART;
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 500) {
log("sample timeout");
return -1;
return 0xffff;
}
}
debug("init done");
/* create the device node */
return CDev::init();
}
int
ADC::ioctl(file *filp, int cmd, unsigned long arg)
{
return -ENOTTY;
}
ssize_t
ADC::read(file *filp, char *buffer, size_t len)
{
const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
if (len > maxsize)
len = maxsize;
/* block interrupts while copying samples to avoid racing with an update */
irqstate_t flags = irqsave();
memcpy(buffer, _samples, len);
irqrestore(flags);
return len;
}
int
ADC::open_first(struct file *filp)
{
/* get fresh data */
_tick();
/* and schedule regular updates */
hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
return 0;
}
int
ADC::close_last(struct file *filp)
{
hrt_cancel(&_call);
return 0;
}
void
ADC::_tick_trampoline(void *arg)
{
((ADC *)arg)->_tick();
}
void
ADC::_tick()
{
/* scan the channel set and sample each */
for (unsigned i = 0; i < _channel_count; i++)
_samples[i].am_data = _sample(_samples[i].am_channel);
}
uint16_t
ADC::_sample(unsigned channel)
{
perf_begin(_sample_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
rCR2 |= ADC_CR2_SWSTART;
/* wait for the conversion to complete */
hrt_abstime now = hrt_absolute_time();
while (!(rSR & ADC_SR_EOC)) {
/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
if ((hrt_absolute_time() - now) > 50) {
log("sample timeout");
return 0xffff;
}
}
/* read the result and clear EOC */
uint16_t result = rDR;
perf_end(_sample_perf);
return result;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int adc_main(int argc, char *argv[]);
namespace
{
ADC *g_adc;
void
test(void)
{
int fd = open(ADC_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "can't open ADC device");
for (unsigned i = 0; i < 50; i++) {
adc_msg_s data[8];
ssize_t count = read(fd, data, sizeof(data));
if (count < 0)
errx(1, "read error");
unsigned channels = count / sizeof(data[0]);
for (unsigned j = 0; j < channels; j++) {
printf ("%d: %u ", data[j].am_channel, data[j].am_data);
}
printf("\n");
usleep(500000);
}
exit(0);
}
}
int
adc_main(int argc, char *argv[])
{
if (g_adc == nullptr) {
/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
if (g_adc == nullptr)
errx(1, "couldn't allocate the ADC driver");
if (g_adc->init() != OK) {
delete g_adc;
errx(1, "ADC init failed");
}
}
if (argc > 1) {
if (!strcmp(argv[1], "test"))
test();
}
exit(0);
}

158
apps/examples/Kconfig
View File

@ -3,206 +3,60 @@
# see misc/tools/kconfig-language.txt.
#
menu "ADC Example"
source "$APPSDIR/examples/adc/Kconfig"
endmenu
menu "Buttons Example"
source "$APPSDIR/examples/buttons/Kconfig"
endmenu
menu "CAN Example"
source "$APPSDIR/examples/can/Kconfig"
endmenu
menu "USB CDC/ACM Class Driver Example"
source "$APPSDIR/examples/cdcacm/Kconfig"
endmenu
menu "USB composite Class Driver Example"
source "$APPSDIR/examples/composite/Kconfig"
endmenu
menu "DHCP Server Example"
source "$APPSDIR/examples/cxxtest/Kconfig"
source "$APPSDIR/examples/dhcpd/Kconfig"
endmenu
menu "FTP Client Example"
source "$APPSDIR/examples/elf/Kconfig"
source "$APPSDIR/examples/ftpc/Kconfig"
endmenu
menu "FTP Server Example"
source "$APPSDIR/examples/ftpd/Kconfig"
endmenu
menu "\"Hello, World!\" Example"
source "$APPSDIR/examples/hello/Kconfig"
endmenu
menu "\"Hello, World!\" C++ Example"
source "$APPSDIR/examples/helloxx/Kconfig"
endmenu
menu "USB HID Keyboard Example"
source "$APPSDIR/examples/json/Kconfig"
source "$APPSDIR/examples/hidkbd/Kconfig"
endmenu
menu "IGMP Example"
source "$APPSDIR/examples/keypadtest/Kconfig"
source "$APPSDIR/examples/igmp/Kconfig"
endmenu
menu "LCD Read/Write Example"
source "$APPSDIR/examples/lcdrw/Kconfig"
endmenu
menu "Memory Management Example"
source "$APPSDIR/examples/mm/Kconfig"
endmenu
menu "File System Mount Example"
source "$APPSDIR/examples/mount/Kconfig"
endmenu
menu "FreeModBus Example"
source "$APPSDIR/examples/modbus/Kconfig"
endmenu
menu "Network Test Example"
source "$APPSDIR/examples/nettest/Kconfig"
endmenu
menu "NuttShell (NSH) Example"
source "$APPSDIR/examples/nsh/Kconfig"
endmenu
menu "NULL Example"
source "$APPSDIR/examples/null/Kconfig"
endmenu
menu "NX Graphics Example"
source "$APPSDIR/examples/nx/Kconfig"
endmenu
menu "NxConsole Example"
source "$APPSDIR/examples/nxconsole/Kconfig"
endmenu
menu "NXFFS File System Example"
source "$APPSDIR/examples/nxffs/Kconfig"
endmenu
menu "NXFLAT Example"
source "$APPSDIR/examples/nxflat/Kconfig"
endmenu
menu "NX Graphics \"Hello, World!\" Example"
source "$APPSDIR/examples/nxhello/Kconfig"
endmenu
menu "NX Graphics image Example"
source "$APPSDIR/examples/nximage/Kconfig"
endmenu
menu "NX Graphics lines Example"
source "$APPSDIR/examples/nxlines/Kconfig"
endmenu
menu "NX Graphics Text Example"
source "$APPSDIR/examples/nxtext/Kconfig"
endmenu
menu "OS Test Example"
source "$APPSDIR/examples/ostest/Kconfig"
endmenu
menu "Pascal \"Hello, World!\"example"
source "$APPSDIR/examples/pashello/Kconfig"
endmenu
menu "Pipe Example"
source "$APPSDIR/examples/pipe/Kconfig"
endmenu
menu "Poll Example"
source "$APPSDIR/examples/poll/Kconfig"
endmenu
menu "Pulse Width Modulation (PWM) Example"
source "$APPSDIR/examples/pwm/Kconfig"
endmenu
menu "Quadrature Encoder Example"
source "$APPSDIR/examples/qencoder/Kconfig"
endmenu
menu "RGMP Example"
source "$APPSDIR/examples/relays/Kconfig"
source "$APPSDIR/examples/rgmp/Kconfig"
endmenu
menu "ROMFS Example"
source "$APPSDIR/examples/romfs/Kconfig"
endmenu
menu "sendmail Example"
source "$APPSDIR/examples/sendmail/Kconfig"
endmenu
menu "Serial Loopback Example"
source "$APPSDIR/examples/serloop/Kconfig"
endmenu
menu "Telnet Daemon Example"
source "$APPSDIR/examples/telnetd/Kconfig"
endmenu
menu "THTTPD Web Server Example"
source "$APPSDIR/examples/thttpd/Kconfig"
endmenu
menu "TIFF Generation Example"
source "$APPSDIR/examples/tiff/Kconfig"
endmenu
menu "Touchscreen Example"
source "$APPSDIR/examples/touchscreen/Kconfig"
endmenu
menu "UDP Example"
source "$APPSDIR/examples/udp/Kconfig"
endmenu
menu "UDP Discovery Daemon Example"
source "$APPSDIR/examples/discover/Kconfig"
endmenu
menu "uIP Web Server Example"
source "$APPSDIR/examples/uip/Kconfig"
endmenu
menu "USB Serial Test Example"
source "$APPSDIR/examples/usbserial/Kconfig"
endmenu
menu "USB Mass Storage Class Example"
source "$APPSDIR/examples/usbstorage/Kconfig"
endmenu
menu "USB Serial Terminal Example"
source "$APPSDIR/examples/usbterm/Kconfig"
endmenu
menu "Watchdog timer Example"
source "$APPSDIR/examples/watchdog/Kconfig"
endmenu
menu "wget Example"
source "$APPSDIR/examples/wget/Kconfig"
endmenu
menu "WLAN Example"
source "$APPSDIR/examples/wgetjson/Kconfig"
source "$APPSDIR/examples/wlan/Kconfig"
endmenu
menu "XML RPC Example"
source "$APPSDIR/examples/xmlrpc/Kconfig"
endmenu

28
apps/examples/Make.defs
View File

@ -54,6 +54,10 @@ ifeq ($(CONFIG_EXAMPLES_COMPOSITE),y)
CONFIGURED_APPS += examples/composite
endif
ifeq ($(CONFIG_EXAMPLES_CXXTEST),y)
CONFIGURED_APPS += examples/cxxtest
endif
ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
CONFIGURED_APPS += examples/dhcpd
endif
@ -62,6 +66,10 @@ ifeq ($(CONFIG_EXAMPLES_DISCOVER),y)
CONFIGURED_APPS += examples/discover
endif
ifeq ($(CONFIG_EXAMPLES_ELF),y)
CONFIGURED_APPS += examples/elf
endif
ifeq ($(CONFIG_EXAMPLES_FTPC),y)
CONFIGURED_APPS += examples/ftpc
endif
@ -86,6 +94,14 @@ ifeq ($(CONFIG_EXAMPLES_IGMP),y)
CONFIGURED_APPS += examples/igmp
endif
ifeq ($(CONFIG_EXAMPLES_JSON),y)
CONFIGURED_APPS += examples/json
endif
ifeq ($(CONFIG_EXAMPLES_KEYPADTEST),y)
CONFIGURED_APPS += examples/keypadtest
endif
ifeq ($(CONFIG_EXAMPLES_LCDRW),y)
CONFIGURED_APPS += examples/lcdrw
endif
@ -94,6 +110,10 @@ ifeq ($(CONFIG_EXAMPLES_MM),y)
CONFIGURED_APPS += examples/mm
endif
ifeq ($(CONFIG_EXAMPLES_MODBUS),y)
CONFIGURED_APPS += examples/modbus
endif
ifeq ($(CONFIG_EXAMPLES_MOUNT),y)
CONFIGURED_APPS += examples/mount
endif
@ -166,6 +186,10 @@ ifeq ($(CONFIG_EXAMPLES_QENCODER),y)
CONFIGURED_APPS += examples/qencoder
endif
ifeq ($(CONFIG_EXAMPLES_RELAYS),y)
CONFIGURED_APPS += examples/relays
endif
ifeq ($(CONFIG_EXAMPLES_RGMP),y)
CONFIGURED_APPS += examples/rgmp
endif
@ -226,6 +250,10 @@ ifeq ($(CONFIG_EXAMPLES_WGET),y)
CONFIGURED_APPS += examples/wget
endif
ifeq ($(CONFIG_EXAMPLES_WGETJSON),y)
CONFIGURED_APPS += examples/wgetjson
endif
ifeq ($(CONFIG_EXAMPLES_WLAN),y)
CONFIGURED_APPS += examples/wlan
endif

49
apps/examples/Makefile
View File

@ -37,12 +37,12 @@
# Sub-directories
SUBDIRS = adc buttons can cdcacm composite dhcpd discover ftpc ftpd hello
SUBDIRS += helloxx hidkbd igmp lcdrw mm modbus mount nettest nsh null nx
SUBDIRS += nxconsole nxffs nxflat nxhello nximage nxlines nxtext ostest
SUBDIRS += pashello pipe poll pwm qencoder rgmp romfs serloop telnetd
SUBDIRS += thttpd tiff touchscreen udp uip usbserial sendmail usbstorage
SUBDIRS += usbterm watchdog wget wlan
SUBDIRS = adc buttons can cdcacm composite cxxtest dhcpd discover elf ftpc
SUBDIRS += ftpd hello helloxx hidkbd igmp json keypadtest lcdrw mm modbus mount
SUBDIRS += nettest nsh null nx nxconsole nxffs nxflat nxhello nximage
SUBDIRS += nxlines nxtext ostest pashello pipe poll pwm qencoder relays
SUBDIRS += rgmp romfs serloop telnetd thttpd tiff touchscreen udp uip
SUBDIRS += usbserial sendmail usbstorage usbterm watchdog wget wgetjson wlan
# Sub-directories that might need context setup. Directories may need
# context setup for a variety of reasons, but the most common is because
@ -57,8 +57,8 @@ SUBDIRS += usbterm watchdog wget wlan
CNTXTDIRS = pwm
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
CNTXTDIRS += adc can cdcacm composite discover ftpd dhcpd modbus nettest
CNTXTDIRS += qencoder telnetd watchdog
CNTXTDIRS += adc can cdcacm composite cxxtest dhcpd discover ftpd json keypadtest
CNTXTDIRS += modbus nettest nxlines relays qencoder telnetd watchdog wgetjson
endif
ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y)
@ -79,9 +79,6 @@ endif
ifeq ($(CONFIG_EXAMPLES_NXIMAGE_BUILTIN),y)
CNTXTDIRS += nximage
endif
ifeq ($(CONFIG_EXAMPLES_LINES_BUILTIN),y)
CNTXTDIRS += nxlines
endif
ifeq ($(CONFIG_EXAMPLES_NXTEXT_BUILTIN),y)
CNTXTDIRS += nxtext
endif
@ -105,27 +102,25 @@ all: nothing
.PHONY: nothing context depend clean distclean
define SDIR_template
$(1)_$(2):
$(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
endef
$(foreach SDIR, $(CNTXTDIRS), $(eval $(call SDIR_template,$(SDIR),context)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),depend)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),clean)))
$(foreach SDIR, $(SUBDIRS), $(eval $(call SDIR_template,$(SDIR),distclean)))
nothing:
context:
@for dir in $(CNTXTDIRS) ; do \
$(MAKE) -C $$dir context TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
context: $(foreach SDIR, $(CNTXTDIRS), $(SDIR)_context)
depend:
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
depend: $(foreach SDIR, $(SUBDIRS), $(SDIR)_depend)
clean:
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
clean: $(foreach SDIR, $(SUBDIRS), $(SDIR)_clean)
distclean: clean
@for dir in $(SUBDIRS) ; do \
$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
distclean: clean $(foreach SDIR, $(SUBDIRS), $(SDIR)_distclean)
-include Make.dep

174
apps/examples/README.txt
View File

@ -239,6 +239,29 @@ examples/composite
CONFIG_EXAMPLES_COMPOSITE_TRACEINTERRUPTS
Show interrupt-related events.
examples/cxxtest
^^^^^^^^^^^^^^^^
This is a test of the C++ standard library. At present a port of the uClibc++
C++ library is available. Due to licensinging issues, the uClibc++ C++ library
is not included in the NuttX source tree by default, but must be installed
(see misc/uClibc++/README.txt for installation).
The NuttX setting that are required include:
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_UCLIBCXX=y
Additional uClibc++ settings may be required in your build environment.
The uClibc++ test includes simple test of:
- iostreams,
- STL,
- RTTI, and
- Exceptions
examples/dhcpd
^^^^^^^^^^^^^^
@ -297,6 +320,68 @@ examples/discover
CONFIG_EXAMPLES_DISCOVER_DRIPADDR - Router IP address
CONFIG_EXAMPLES_DISCOVER_NETMASK - Network Mask
examples/elf
^^^^^^^^^^^^
This example builds a small ELF loader test case. This includes several
test programs under examples/elf tests. These tests are build using
the relocatable ELF format and installed in a ROMFS file system. At run time,
each program in the ROMFS file system is executed. Requires CONFIG_ELF.
Other configuration options:
CONFIG_EXAMPLES_ELF_DEVMINOR - The minor device number of the ROMFS block.
For example, the N in /dev/ramN. Used for registering the RAM block driver
that will hold the ROMFS file system containing the ELF executables to be
tested. Default: 0
CONFIG_EXAMPLES_ELF_DEVPATH - The path to the ROMFS block driver device. This
must match EXAMPLES_ELF_DEVMINOR. Used for registering the RAM block driver
that will hold the ROMFS file system containing the ELF executables to be
tested. Default: "/dev/ram0"
NOTES:
1. CFLAGS should be provided in CELFFLAGS. RAM and FLASH memory regions
may require long allcs. For ARM, this might be:
CELFFLAGS = $(CFLAGS) -mlong-calls
Similarly for C++ flags which must be provided in CXXELFFLAGS.
2. Your top-level nuttx/Make.defs file must alos include an approproate definition,
LDELFFLAGS, to generate a relocatable ELF object. With GNU LD, this should
include '-r' and '-e main' (or _main on some platforms).
LDELFFLAGS = -r -e main
If you use GCC to link, you make also need to include '-nostdlib' or
'-nostartfiles' and '-nodefaultlibs'.
3. This example also requires genromfs. genromfs can be build as part of the
nuttx toolchain. Or can built from the genromfs sources that can be found
at misc/tools/genromfs-0.5.2.tar.gz. In any event, the PATH variable must
include the path to the genromfs executable.
4. ELF size: The ELF files in this example are, be default, quite large
because they include a lot of "build garbage". You can greatly reduce the
size of the ELF binaries are using the 'objcopy --strip-unneeded' command to
remove un-necessary information from the ELF files.
5. Simulator. You cannot use this example with the the NuttX simulator on
Cygwin. That is because the Cygwin GCC does not generate ELF file but
rather some Windows-native binary format.
If you really want to do this, you can create a NuttX x86 buildroot toolchain
and use that be build the ELF executables for the ROMFS file system.
6. Linker scripts. You might also want to use a linker scripts to combine
sections better. An example linker script is at nuttx/binfmt/libelf/gnu-elf.ld.
That example might have to be tuned for your particular linker output to
position additional sections correctly. The GNU LD LDELFFLAGS then might
be:
LDELFFLAGS = -r -e main -T$(TOPDIR)/binfmt/libelf/gnu-elf.ld
examples/ftpc
^^^^^^^^^^^^^
@ -482,6 +567,32 @@ examples/igmp
CONFIGURED_APPS += uiplib
examples/json
^^^^^^^^^^^^^
This example exercises the cJSON implementation at apps/netutils/json.
This example contains logic taken from the cJSON project:
http://sourceforge.net/projects/cjson/
The example corresponds to SVN revision r42 (with lots of changes for
NuttX coding standards). As of r42, the SVN repository was last updated
on 2011-10-10 so I presume that the code is stable and there is no risk
of maintaining duplicate logic in the NuttX repository.
examples/keypadtest
^^^^^^^^^^^^^^^^^^^
This is a generic keypad test example. It is similar to the USB hidkbd
example, but makes no assumptions about the underlying keyboard interface.
It uses the interfaces of include/nuttx/input/keypad.h.
CONFIG_EXAMPLES_KEYPADTEST - Selects the keypadtest example (only need
if the mconf/Kconfig tool is used.
CONFIG_EXAMPLES_KEYPAD_DEVNAME - The name of the keypad device that will
be opened in order to perform the keypad test. Default: "/dev/keypad"
examples/lcdrw
^^^^^^^^^^^^^^
@ -496,6 +607,11 @@ examples/lcdrw
* CONFIG_EXAMPLES_LDCRW_YRES
LCD Y resolution. Default: 320
NOTE: This test exercises internal lcd driver interfaces. As such, it
relies on internal OS interfaces that are not normally available to a
user-space program. As a result, this example cannot be used if a
NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL).
examples/mm
^^^^^^^^^^^
@ -838,8 +954,6 @@ examplex/nxlines
The following configuration options can be selected:
CONFIG_EXAMPLES_NXLINES_BUILTIN -- Build the NXLINES example as a "built-in"
that can be executed from the NSH command line
CONFIG_EXAMPLES_NXLINES_VPLANE -- The plane to select from the frame-
buffer driver for use in the test. Default: 0
CONFIG_EXAMPLES_NXLINES_DEVNO - The LCD device to select from the LCD
@ -877,6 +991,9 @@ examplex/nxlines
FAR struct fb_vtable_s *up_nxdrvinit(unsigned int devno);
#endif
CONFIG_NSH_BUILTIN_APPS - Build the NX lines examples as an NSH built-in
function.
examples/nxtext
^^^^^^^^^^^^^^^
@ -984,6 +1101,17 @@ examples/ostest
Specifies the number of threads to create in the barrier
test. The default is 8 but a smaller number may be needed on
systems without sufficient memory to start so many threads.
* CONFIG_EXAMPLES_OSTEST_RR_RANGE
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
This value specifies the end of search range and together with number of runs
allows to configure the length of this test - it should last at least a few
tens of seconds. Allowed values [1; 32767], default 10000
* CONFIG_EXAMPLES_OSTEST_RR_RUNS
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
examples/pashello
^^^^^^^^^^^^^^^^^
@ -1110,17 +1238,28 @@ examples/qencoder
Specific configuration options for this example include:
CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default:
/dev/qe0
CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS
is defined, then the number of samples is provided on the command line
and this value is ignored. Otherwise, this number of samples is
collected and the program terminates. Default: Samples are collected
indefinitely.
CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in
milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS
is defined, then this value is the default delay if no other delay is
provided on the command line. Default: 100 milliseconds
CONFIG_EXAMPLES_QENCODER_DEVPATH - The path to the QE device. Default:
/dev/qe0
CONFIG_EXAMPLES_QENCODER_NSAMPLES - If CONFIG_NSH_BUILTIN_APPS
is defined, then the number of samples is provided on the command line
and this value is ignored. Otherwise, this number of samples is
collected and the program terminates. Default: Samples are collected
indefinitely.
CONFIG_EXAMPLES_QENCODER_DELAY - This value provides the delay (in
milliseonds) between each sample. If CONFIG_NSH_BUILTIN_APPS
is defined, then this value is the default delay if no other delay is
provided on the command line. Default: 100 milliseconds
examples/relays
^^^^^^^^^^^^^^^
Requires CONFIG_ARCH_RELAYS.
Contributed by Darcy Gong.
NOTE: This test exercises internal relay driver interfaces. As such, it
relies on internal OS interfaces that are not normally available to a
user-space program. As a result, this example cannot be used if a
NuttX is built as a protected, supervisor kernel (CONFIG_NUTTX_KERNEL).
examples/rgmp
^^^^^^^^^^^^^
@ -1672,7 +1811,16 @@ examples/wget
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webclient
examples/wget
^^^^^^^^^^^^^
Uses wget to get a JSON encoded file, then decodes the file.
CONFIG_EXAMPLES_WDGETJSON_MAXSIZE - Max. JSON Buffer Size
CONFIG_EXAMPLES_EXAMPLES_WGETJSON_URL - wget URL
examples/xmlrpc
^^^^^^^^^^^^^^^
This example exercises the "Embeddable Lightweight XML-RPC Server" which
is discussed at:

23
apps/examples/adc/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/adc/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -90,16 +92,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

7
apps/examples/adc/adc_main.c
View File

@ -289,7 +289,7 @@ int adc_main(int argc, char *argv[])
{
message("adc_main: open %s failed: %d\n", g_adcstate.devpath, errno);
errval = 2;
goto errout_with_dev;
goto errout;
}
/* Now loop the appropriate number of times, displaying the collected
@ -357,6 +357,11 @@ int adc_main(int argc, char *argv[])
}
}
close(fd);
return OK;
/* Error exits */
errout_with_dev:
close(fd);

23
apps/examples/buttons/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/buttons/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -90,16 +92,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

23
apps/examples/can/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/can/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -90,16 +92,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

21
apps/examples/cdcacm/Makefile
View File

@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -80,9 +84,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -93,17 +95,18 @@ $(COBJS): %$(OBJEXT): %.c
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

10
apps/fixedwing_control/Makefile → apps/examples/control_demo/Makefile
View File

@ -32,13 +32,11 @@
############################################################################
#
# fixedwing_control Application
# Basic example application
#
APPNAME = fixedwing_control
PRIORITY = SCHED_PRIORITY_MAX - 1
STACKSIZE = 4096
CSRCS = fixedwing_control.c
APPNAME = control_demo
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk

168
apps/examples/control_demo/control_demo.cpp Normal file
View File

@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file control_demo.cpp
* Demonstration of control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <math.h>
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 control_demo_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int control_demo_thread_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* 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: control_demo {start|stop|status} [-p <additional params>]\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 control_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
control_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
} else {
printf("\tcontrol_demo app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
}
printf("[control_demo] exiting.\n");
thread_running = false;
return 0;
}
void test()
{
printf("beginning control lib test\n");
control::basicBlocksTest();
}

63
apps/examples/control_demo/params.c Normal file
View File

@ -0,0 +1,63 @@
#include <systemlib/param/param.h>
// currently tuned for easystar from arkhangar in HIL
//https://github.com/arktools/arkhangar
// 16 is max name length
// gyro low pass filter
PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
// yaw washout
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
// stabilization mode
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
// psi -> phi -> p
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll
PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg
// velocity -> theta
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle
// theta -> q
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
// h -> thr
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
// crosstrack
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain
// speed command
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)

21
apps/examples/hello/Makefile
View File

@ -54,10 +54,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -90,16 +92,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

25
apps/examples/helloxx/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/helloxx/Makefile
#
# Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -50,10 +50,14 @@ CXXOBJS = $(CXXSRCS:.cxx=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS) $(CXXSRCS)
OBJS = $(AOBJS) $(COBJS) $(CXXOBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -69,7 +73,7 @@ STACKSIZE = 2048
VPATH =
all: .built
.PHONY: clean depend disclean chkcxx
.PHONY: clean depend distclean chkcxx
chkcxx:
ifneq ($(CONFIG_HAVE_CXX),y)
@ -93,9 +97,7 @@ $(CXXOBJS): %$(OBJEXT): %.cxx
$(call COMPILEXX, $<, $@)
.built: chkcxx $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -107,16 +109,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

761
apps/examples/kalman_demo/KalmanNav.cpp Normal file
View File

@ -0,0 +1,761 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.cpp
*
* kalman filter navigation code
*/
#include <poll.h>
#include "KalmanNav.hpp"
// constants
// Titterton pg. 52
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
static const float R0 = 6378137.0f; // earth radius, m
static const float g0 = 9.806f; // standard gravitational accel. m/s^2
static const int8_t ret_ok = 0; // no error in function
static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// ekf matrices
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
// attitude measurement ekf matrices
HAtt(6, 9),
RAtt(6, 6),
// position measurement ekf matrices
HPos(5, 9),
RPos(5, 5),
// attitude representations
C_nb(),
q(),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
_rMag(this, "R_MAG"),
_rGpsVel(this, "R_GPS_VEL"),
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_attitudeInitialized(false),
_positionInitialized(false),
_attitudeInitCounter(0)
{
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 0.01f;
P = P0;
// initial state
phi = 0.0f;
theta = 0.0f;
psi = 0.0f;
vN = 0.0f;
vE = 0.0f;
vD = 0.0f;
lat = 0.0f;
lon = 0.0f;
alt = 0.0f;
// initialize quaternions
q = Quaternion(EulerAngles(phi, theta, psi));
// initialize dcm
C_nb = Dcm(q);
// HPos is constant
HPos(0, 3) = 1.0f;
HPos(1, 4) = 1.0f;
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
HPos(4, 8) = 1.0f;
// initialize all parameters
updateParams();
}
void KalmanNav::update()
{
using namespace math;
struct pollfd fds[1];
fds[0].fd = _sensors.getHandle();
fds[0].events = POLLIN;
// poll for new data
int ret = poll(fds, 1, 1000);
if (ret < 0) {
// XXX this is seriously bad - should be an emergency
return;
} else if (ret == 0) { // timeout
return;
}
// get new timestamp
uint64_t newTimeStamp = hrt_absolute_time();
// check updated subscriptions
if (_param_update.updated()) updateParams();
bool gpsUpdate = _gps.updated();
bool sensorsUpdate = _sensors.updated();
// get new information from subscriptions
// this clears update flag
updateSubscriptions();
// initialize attitude when sensors online
if (!_attitudeInitialized && sensorsUpdate &&
_sensors.accelerometer_counter > 10 &&
_sensors.gyro_counter > 10 &&
_sensors.magnetometer_counter > 10) {
if (correctAtt() == ret_ok) _attitudeInitCounter++;
if (_attitudeInitCounter > 100) {
printf("[kalman_demo] initialized EKF attitude\n");
printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
double(phi), double(theta), double(psi));
_attitudeInitialized = true;
}
}
// initialize position when gps received
if (!_positionInitialized &&
_attitudeInitialized && // wait for attitude first
gpsUpdate &&
_gps.fix_type > 2 &&
_gps.counter_pos_valid > 10) {
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
_positionInitialized = true;
printf("[kalman_demo] initialized EKF state with GPS\n");
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
lat, lon, alt);
}
// prediciton step
// using sensors timestamp so we can account for packet lag
float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dt));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (_positionInitialized && gpsUpdate) {
correctPos();
}
// attitude correction step
if (_attitudeInitialized // initialized
&& sensorsUpdate // new data
&& _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz
) {
_attTimeStamp = _sensors.timestamp;
correctAtt();
}
// publication
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
_pubTimeStamp = newTimeStamp;
updatePublications();
}
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
}
void KalmanNav::updatePublications()
{
using namespace math;
// position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp;
_pos.valid = true;
_pos.lat = getLatDegE7();
_pos.lon = getLonDegE7();
_pos.alt = float(alt);
_pos.relative_alt = float(alt); // TODO, make relative
_pos.vx = vN;
_pos.vy = vE;
_pos.vz = vD;
_pos.hdg = psi;
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
_att.pitch = theta;
_att.yaw = psi;
_att.rollspeed = _sensors.gyro_rad_s[0];
_att.pitchspeed = _sensors.gyro_rad_s[1];
_att.yawspeed = _sensors.gyro_rad_s[2];
// TODO, add gyro offsets to filter
_att.rate_offsets[0] = 0.0f;
_att.rate_offsets[1] = 0.0f;
_att.rate_offsets[2] = 0.0f;
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
_att.R[i][j] = C_nb(i, j);
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
_att.R_valid = true;
_att.q_valid = true;
_att.counter = _navFrames;
// selectively update publications,
// do NOT call superblock do-all method
if (_positionInitialized)
_pos.update();
if (_attitudeInitialized)
_att.update();
}
int KalmanNav::predictState(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
}
// C_nb update
C_nb = Dcm(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
}
// position prediction
if (_positionInitialized) {
// neglects angular deflections in local gravity
// see Titerton pg. 70
float R = R0 + float(alt);
float LDot = vN / R;
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL;
// rectangular integration
vN += vNDot * dt;
vE += vEDot * dt;
vD += vDDot * dt;
lat += double(LDot * dt);
lon += double(lDot * dt);
alt += double(-vD * dt);
}
return ret_ok;
}
int KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
// trig
float sinL = sinf(lat);
float cosL = cosf(lat);
float cosLSq = cosL * cosL;
float tanL = tanf(lat);
// prepare for matrix
float R = R0 + float(alt);
float RSq = R * R;
// F Matrix
// Titterton pg. 291
F(0, 1) = -(omega * sinL + vE * tanL / R);
F(0, 2) = vN / R;
F(0, 4) = 1.0f / R;
F(0, 6) = -omega * sinL;
F(0, 8) = -vE / RSq;
F(1, 0) = omega * sinL + vE * tanL / R;
F(1, 2) = omega * cosL + vE / R;
F(1, 3) = -1.0f / R;
F(1, 8) = vN / RSq;
F(2, 0) = -vN / R;
F(2, 1) = -omega * cosL - vE / R;
F(2, 4) = -tanL / R;
F(2, 6) = -omega * cosL - vE / (R * cosLSq);
F(2, 8) = vE * tanL / RSq;
F(3, 1) = -fD;
F(3, 2) = fE;
F(3, 3) = vD / R;
F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
F(3, 5) = vN / R;
F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
F(4, 0) = fD;
F(4, 2) = -fN;
F(4, 3) = 2 * omega * sinL + vE * tanL / R;
F(4, 4) = (vN * tanL + vD) / R;
F(4, 5) = 2 * omega * cosL + vE / R;
F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
vN * vE / (R * cosLSq);
F(4, 8) = -vE * (vN * tanL + vD) / RSq;
F(5, 0) = -fE;
F(5, 1) = fN;
F(5, 3) = -2 * vN / R;
F(5, 4) = -2 * (omega * cosL + vE / R);
F(5, 6) = 2 * omega * vE * sinL;
F(5, 8) = (vN * vN + vE * vE) / RSq;
F(6, 3) = 1 / R;
F(6, 8) = -vN / RSq;
F(7, 4) = 1 / (R * cosL);
F(7, 6) = vE * tanL / (R * cosL);
F(7, 8) = -vE / (cosL * RSq);
F(8, 5) = -1;
// G Matrix
// Titterton pg. 291
G(0, 0) = -C_nb(0, 0);
G(0, 1) = -C_nb(0, 1);
G(0, 2) = -C_nb(0, 2);
G(1, 0) = -C_nb(1, 0);
G(1, 1) = -C_nb(1, 1);
G(1, 2) = -C_nb(1, 2);
G(2, 0) = -C_nb(2, 0);
G(2, 1) = -C_nb(2, 1);
G(2, 2) = -C_nb(2, 2);
G(3, 3) = C_nb(0, 0);
G(3, 4) = C_nb(0, 1);
G(3, 5) = C_nb(0, 2);
G(4, 3) = C_nb(1, 0);
G(4, 4) = C_nb(1, 1);
G(4, 5) = C_nb(1, 2);
G(5, 3) = C_nb(2, 0);
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous predictioon equations
// for discrte time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
return ret_ok;
}
int KalmanNav::correctAtt()
{
using namespace math;
// trig
float cosPhi = cosf(phi);
float cosTheta = cosf(theta);
float cosPsi = cosf(psi);
float sinPhi = sinf(phi);
float sinTheta = sinf(theta);
float sinPsi = sinf(psi);
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// mag predicted measurement
// choosing some typical magnetic field properties,
// TODO dip/dec depend on lat/ lon/ time
float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
float bN = cosf(dip) * cosf(dec);
float bE = cosf(dip) * sinf(dec);
float bD = sinf(dip);
Vector3 bNav(bN, bE, bD);
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
// accel measurement
Vector3 zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.norm();
zAccel = zAccel.unit();
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
if (ignoreAccel) {
RAttAdjust(3, 3) = 1.0e10;
RAttAdjust(4, 4) = 1.0e10;
RAttAdjust(5, 5) = 1.0e10;
} else {
//printf("correcting attitude with accel\n");
}
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
// combined measurement
Vector zAtt(6);
Vector zAttHat(6);
for (int i = 0; i < 3; i++) {
zAtt(i) = zMag(i);
zAtt(i + 3) = zAccel(i);
zAttHat(i) = zMagHat(i);
zAttHat(i + 3) = zAccelHat(i);
}
// HMag , HAtt (0-2,:)
float tmp1 =
cosPsi * cosTheta * bN +
sinPsi * cosTheta * bE -
sinTheta * bD;
HAtt(0, 1) = -(
cosPsi * sinTheta * bN +
sinPsi * sinTheta * bE +
cosTheta * bD
);
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
HAtt(1, 0) =
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
cosPhi * cosTheta * bD;
HAtt(1, 1) = sinPhi * tmp1;
HAtt(1, 2) = -(
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
);
HAtt(2, 0) = -(
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
(sinPhi * cosTheta) * bD
);
HAtt(2, 1) = cosPhi * tmp1;
HAtt(2, 2) = -(
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
);
// HAccel , HAtt (3-5,:)
HAtt(3, 1) = cosTheta;
HAtt(4, 0) = -cosPhi * cosTheta;
HAtt(4, 1) = sinPhi * sinTheta;
HAtt(5, 0) = sinPhi * cosTheta;
HAtt(5, 1) = cosPhi * sinTheta;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Vector y = zAtt - zAttHat; // residual
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in att correction\n");
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
if (!ignoreAccel) {
phi += xCorrect(PHI);
theta += xCorrect(THETA);
}
psi += xCorrect(PSI);
// attitude also affects nav velocities
if (_positionInitialized) {
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
}
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse() * y);
if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
}
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
return ret_ok;
}
int KalmanNav::correctPos()
{
using namespace math;
// residual
Vector y(5);
y(0) = _gps.vel_n - vN;
y(1) = _gps.vel_e - vE;
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
float val = xCorrect(i);
if (isnan(val) || isinf(val)) {
// abort correction and return
printf("[kalman_demo] numerical failure in gps correction\n");
// fallback to GPS
vN = _gps.vel_n;
vE = _gps.vel_e;
vD = _gps.vel_d;
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
// reset P matrix to P0
P = P0;
return ret_error;
}
}
// correct state
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += double(xCorrect(ALT));
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
if (beta > _faultPos.get()) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
double(y(3) / sqrtf(RPos(3, 3))),
double(y(4) / sqrtf(RPos(4, 4))));
}
return ret_ok;
}
void KalmanNav::updateParams()
{
using namespace math;
using namespace control;
SuperBlock::updateParams();
// gyro noise
V(0, 0) = _vGyro.get(); // gyro x, rad/s
V(1, 1) = _vGyro.get(); // gyro y
V(2, 2) = _vGyro.get(); // gyro z
// accel noise
V(3, 3) = _vAccel.get(); // accel x, m/s^2
V(4, 4) = _vAccel.get(); // accel y
V(5, 5) = _vAccel.get(); // accel z
// magnetometer noise
float noiseMin = 1e-6f;
float noiseMagSq = _rMag.get() * _rMag.get();
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
RAtt(0, 0) = noiseMagSq; // normalized direction
RAtt(1, 1) = noiseMagSq;
RAtt(2, 2) = noiseMagSq;
// accelerometer noise
float noiseAccelSq = _rAccel.get() * _rAccel.get();
// bound noise to prevent singularities
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
RAtt(3, 3) = noiseAccelSq; // normalized direction
RAtt(4, 4) = noiseAccelSq;
RAtt(5, 5) = noiseAccelSq;
// gps noise
float R = R0 + float(alt);
float cosLSing = cosf(lat);
// prevent singularity
if (fabsf(cosLSing) < 0.01f) {
if (cosLSing > 0) cosLSing = 0.01;
else cosLSing = -0.01;
}
float noiseVel = _rGpsVel.get();
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
float noiseAlt = _rGpsAlt.get();
// bound noise to prevent singularities
if (noiseVel < noiseMin) noiseVel = noiseMin;
if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
if (noiseAlt < noiseMin) noiseAlt = noiseMin;
RPos(0, 0) = noiseVel * noiseVel; // vn
RPos(1, 1) = noiseVel * noiseVel; // ve
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
RPos(4, 4) = noiseAlt * noiseAlt; // h
// XXX, note that RPos depends on lat, so updateParams should
// be called if lat changes significantly
}

179
apps/examples/kalman_demo/KalmanNav.hpp Normal file
View File

@ -0,0 +1,179 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file KalmanNav.hpp
*
* kalman filter navigation code
*/
#pragma once
//#define MATRIX_ASSERT
//#define VECTOR_ASSERT
#include <nuttx/config.h>
#include <mathlib/mathlib.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <controllib/block/UOrbSubscription.hpp>
#include <controllib/block/UOrbPublication.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include <unistd.h>
/**
* Kalman filter navigation class
* http://en.wikipedia.org/wiki/Extended_Kalman_filter
* Discrete-time extended Kalman filter
*/
class KalmanNav : public control::SuperBlock
{
public:
/**
* Constructor
*/
KalmanNav(SuperBlock *parent, const char *name);
/**
* Deconstuctor
*/
virtual ~KalmanNav() {};
/**
* The main callback function for the class
*/
void update();
/**
* Publication update
*/
virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
int predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
int predictStateCovariance(float dt);
/**
* Attitude correction
*/
int correctAtt();
/**
* Position correction
*/
int correctPos();
/**
* Overloaded update parameters
*/
virtual void updateParams();
protected:
// kalman filter
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
math::Matrix G; /**< noise shaping matrix for gyro/accel */
math::Matrix P; /**< state covariance matrix */
math::Matrix P0; /**< initial state covariance matrix */
math::Matrix V; /**< gyro/ accel noise matrix */
math::Matrix HAtt; /**< attitude measurement matrix */
math::Matrix RAtt; /**< attitude measurement noise matrix */
math::Matrix HPos; /**< position measurement jacobian matrix */
math::Matrix RPos; /**< position measurement noise matrix */
// attitude
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
math::Quaternion q; /**< quaternion from body to nav frame */
// subscriptions
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon, alt; /**< lat, lon, alt, radians */
// parameters
control::BlockParam<float> _vGyro; /**< gyro process noise */
control::BlockParam<float> _vAccel; /**< accelerometer process noise */
control::BlockParam<float> _rMag; /**< magnetometer measurement noise */
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParam<float> _g; /**< gravitational constant */
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
// status
bool _attitudeInitialized;
bool _positionInitialized;
uint16_t _attitudeInitCounter;
// accessors
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
};

42
apps/examples/kalman_demo/Makefile Normal file
View File

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

152
apps/examples/kalman_demo/kalman_demo.cpp Normal file
View File

@ -0,0 +1,152 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file kalman_demo.cpp
* Demonstration of control library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.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 kalman_demo_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int kalman_demo_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: kalman_demo {start|stop|status} [-p <additional params>]\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 kalman_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("kalman_demo already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("kalman_demo",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
kalman_demo_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tkalman_demo app is running\n");
} else {
printf("\tkalman_demo app not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int kalman_demo_thread_main(int argc, char *argv[])
{
printf("[kalman_demo] starting\n");
using namespace math;
thread_running = true;
KalmanNav nav(NULL, "KF");
while (!thread_should_exit) {
nav.update();
}
printf("[kalman_demo] exiting.\n");
thread_running = false;
return 0;
}

16
apps/examples/kalman_demo/params.c Normal file
View File

@ -0,0 +1,16 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);

42
apps/examples/math_demo/Makefile Normal file
View File

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

105
apps/examples/math_demo/math_demo.cpp Normal file
View File

@ -0,0 +1,105 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file math_demo.cpp
* Demonstration of math library
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
/**
* Management function.
*/
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
/**
* Test function
*/
void test();
/**
* 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: math_demo {test}\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 math_demo_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "test")) {
test();
exit(0);
}
usage("unrecognized command");
exit(1);
}
void test()
{
printf("beginning math lib test\n");
using namespace math;
vectorTest();
matrixTest();
vector3Test();
eulerAnglesTest();
quaternionTest();
dcmTest();
}

23
apps/examples/mm/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/mm/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

23
apps/examples/mount/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/Makefile
#
# Copyright (C) 2007-2008, 2010-2010 Gregory Nutt. All rights reserved.
# Copyright (C) 2007-2008, 2010-2010, 2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

23
apps/examples/nsh/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/nsh/Makefile
#
# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2007-2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

23
apps/examples/null/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# examples/null/Makefile
#
# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2007-2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

27
apps/examples/ostest/Kconfig
View File

@ -39,4 +39,31 @@ config EXAMPLES_OSTEST_NBARRIER_THREADS
is 8 but a smaller number may be needed on systems without sufficient memory
to start so many threads.
config EXAMPLES_OSTEST_RR_RANGE
int "Round-robin test - end of search range"
default 10000
range 1 32767
---help---
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
This value specifies the end of search range and together with number of runs
allows to configure the length of this test - it should last at least a few
tens of seconds. Allowed values [1; 32767], default 10000
config EXAMPLES_OSTEST_RR_RUNS
int "Round-robin test - number of runs"
default 10
range 1 32767
---help---
During round-robin scheduling test two threads are created. Each of the threads
searches for prime numbers in the configurable range, doing that configurable
number of times.
This value specifies the number of times the thread searches the range for
prime numbers and together with end of search range allows to configure the
length of this test - it should last at least a few tens of seconds. Allowed
values [1; 32767], default 10
endif

21
apps/examples/ostest/Makefile
View File

@ -98,10 +98,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -120,9 +124,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -134,16 +136,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

156
apps/examples/ostest/roundrobin.c
View File

@ -1,7 +1,7 @@
/********************************************************************************
* examples/ostest/roundrobin.c
*
* Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Copyright (C) 2007, 2008, 2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
@ -39,6 +39,7 @@
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include "ostest.h"
#if CONFIG_RR_INTERVAL > 0
@ -47,115 +48,87 @@
* Definitions
********************************************************************************/
/* This number may need to be tuned for different processor speeds. Since these
* arrays must be large to very correct SCHED_RR behavior, this test may require
* too much memory on many targets.
*/
/* This numbers should be tuned for different processor speeds via .config file.
* With default values the test takes about 30s on Cortex-M3 @ 24MHz. With 32767
* range and 10 runs it takes ~320s. */
/* #define CONFIG_NINTEGERS 32768 Takes forever on 60Mhz ARM7 */
#ifndef CONFIG_EXAMPLES_OSTEST_RR_RANGE
# define CONFIG_EXAMPLES_OSTEST_RR_RANGE 10000
# warning "CONFIG_EXAMPLES_OSTEST_RR_RANGE undefined, using default value = 10000"
#elif (CONFIG_EXAMPLES_OSTEST_RR_RANGE < 1) || (CONFIG_EXAMPLES_OSTEST_RR_RANGE > 32767)
# define CONFIG_EXAMPLES_OSTEST_RR_RANGE 10000
# warning "Invalid value of CONFIG_EXAMPLES_OSTEST_RR_RANGE, using default value = 10000"
#endif
#define CONFIG_NINTEGERS 2048
/********************************************************************************
* Private Data
********************************************************************************/
static int prime1[CONFIG_NINTEGERS];
static int prime2[CONFIG_NINTEGERS];
#ifndef CONFIG_EXAMPLES_OSTEST_RR_RUNS
# define CONFIG_EXAMPLES_OSTEST_RR_RUNS 10
# warning "CONFIG_EXAMPLES_OSTEST_RR_RUNS undefined, using default value = 10"
#elif (CONFIG_EXAMPLES_OSTEST_RR_RUNS < 1) || (CONFIG_EXAMPLES_OSTEST_RR_RUNS > 32767)
# define CONFIG_EXAMPLES_OSTEST_RR_RUNS 10
# warning "Invalid value of CONFIG_EXAMPLES_OSTEST_RR_RUNS, using default value = 10"
#endif
/********************************************************************************
* Private Functions
********************************************************************************/
/********************************************************************************
* Name: dosieve
* Name: get_primes
*
* Description
* This implements a "sieve of aristophanes" algorithm for finding prime number.
* Credit for this belongs to someone, but I am not sure who anymore. Anyway,
* the only purpose here is that we need some algorithm that takes a long period
* of time to execute.
*
* This function searches for prime numbers in the most primitive way possible.
********************************************************************************/
static void dosieve(int *prime)
static void get_primes(int *count, int *last)
{
int a,d;
int i;
int j;
int number;
int local_count = 0;
*last = 0; // to make compiler happy
a = 2;
d = a;
for (number = 1; number < CONFIG_EXAMPLES_OSTEST_RR_RANGE; number++)
{
int div;
bool is_prime = true;
for (i = 0; i < CONFIG_NINTEGERS; i++)
for (div = 2; div <= number / 2; div++)
if (number % div == 0)
{
is_prime = false;
break;
}
if (is_prime)
{
prime[i] = i+2;
}
for (i = 1; i < 10; i++)
{
for (j = 0; j < CONFIG_NINTEGERS; j++)
{
d = a + d;
if (d < CONFIG_NINTEGERS)
{
prime[d]=0;
}
}
a++;
d = a;
i++;
}
local_count++;
*last = number;
#if 0 /* We don't really care what the numbers are */
for (i = 0, j= 0; i < CONFIG_NINTEGERS; i++)
{
if (prime[i] != 0)
{
printf(" Prime %d: %d\n", j, prime[i]);
j++;
}
}
printf(" Prime %d: %d\n", local_count, number);
#endif
}
}
*count = local_count;
}
/********************************************************************************
* Name: sieve1
* Name: get_primes_thread
********************************************************************************/
static void *sieve1(void *parameter)
static void *get_primes_thread(void *parameter)
{
int i;
int id = (int)parameter;
int i, count, last;
printf("sieve1 started\n");
printf("get_primes_thread id=%d started, looking for primes < %d, doing %d run(s)\n",
id, CONFIG_EXAMPLES_OSTEST_RR_RANGE, CONFIG_EXAMPLES_OSTEST_RR_RUNS);
for (i = 0; i < 1000; i++)
for (i = 0; i < CONFIG_EXAMPLES_OSTEST_RR_RUNS; i++)
{
dosieve(prime1);
get_primes(&count, &last);
}
printf("sieve1 finished\n");
pthread_exit(NULL);
return NULL; /* To keep some compilers happy */
}
/********************************************************************************
* Name: sieve2
********************************************************************************/
static void *sieve2(void *parameter)
{
int i;
printf("sieve2 started\n");
for (i = 0; i < 1000; i++)
{
dosieve(prime2);
}
printf("sieve2 finished\n");
printf("get_primes_thread id=%d finished, found %d primes, last one was %d\n",
id, count, last);
pthread_exit(NULL);
return NULL; /* To keep some compilers happy */
@ -171,14 +144,13 @@ static void *sieve2(void *parameter)
void rr_test(void)
{
pthread_t sieve1_thread;
pthread_t sieve2_thread;
pthread_t get_primes1_thread;
pthread_t get_primes2_thread;
struct sched_param sparam;
pthread_attr_t attr;
pthread_addr_t result;
int status;
printf("rr_test: Starting sieve1 thread \n");
status = pthread_attr_init(&attr);
if (status != OK)
{
@ -203,29 +175,31 @@ void rr_test(void)
}
else
{
printf("rr_test: Set thread policty to SCHED_RR\n");
printf("rr_test: Set thread policy to SCHED_RR\n");
}
status = pthread_create(&sieve1_thread, &attr, sieve1, NULL);
printf("rr_test: Starting first get_primes_thread\n");
status = pthread_create(&get_primes1_thread, &attr, get_primes_thread, (void*)1);
if (status != 0)
{
printf("rr_test: Error in thread 1 creation, status=%d\n", status);
}
printf("rr_test: Starting sieve1 thread \n");
printf("rr_test: Starting second get_primes_thread\n");
status = pthread_create(&sieve2_thread, &attr, sieve2, NULL);
status = pthread_create(&get_primes2_thread, &attr, get_primes_thread, (void*)2);
if (status != 0)
{
printf("rr_test: Error in thread 2 creation, status=%d\n", status);
}
printf("rr_test: Waiting for sieves to complete -- this should take awhile\n");
printf("rr_test: Waiting for threads to complete -- this should take awhile\n");
printf("rr_test: If RR scheduling is working, they should start and complete at\n");
printf("rr_test: about the same time\n");
pthread_join(sieve2_thread, &result);
pthread_join(sieve1_thread, &result);
pthread_join(get_primes2_thread, &result);
pthread_join(get_primes1_thread, &result);
printf("rr_test: Done\n");
}

23
apps/examples/pipe/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/pipe/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,24 +74,23 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

24
apps/examples/poll/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/poll/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,25 +74,25 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
# Register application
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend host
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
$(call DELFILE, host$(HOSTEXEEXT))
-include Make.dep

23
apps/examples/pwm/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/pwm/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -88,16 +90,17 @@ $(COBJS): %$(OBJEXT): %.c
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

1
apps/examples/pwm/pwm_main.c
View File

@ -48,6 +48,7 @@
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <string.h>
#include <nuttx/pwm.h>

21
apps/examples/qencoder/Makefile
View File

@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -90,16 +92,17 @@ endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

26
apps/examples/romfs/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/romfs/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -61,7 +65,7 @@ ROOTDEPPATH = --dep-path .
VPATH =
all: .built
.PHONY: checkgenromfs clean depend disclean
.PHONY: checkgenromfs clean depend distclean
$(AOBJS): %$(OBJEXT): %.S
$(call ASSEMBLE, $<, $@)
@ -86,26 +90,26 @@ romfs_testdir.h : testdir.img
@xxd -i $< >$@ || { echo "xxd of $< failed" ; exit 1 ; }
.built: romfs_testdir.h $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
# Register application
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend testdir.img
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
$(call DELFILE, testdir.img)
-include Make.dep

23
apps/examples/serloop/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/examples/serloop/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2008, 2010-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -70,26 +74,25 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
context:
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
# Register application
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

21
apps/examples/watchdog/Makefile
View File

@ -48,10 +48,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOTDEPPATH = --dep-path .
@ -76,9 +80,7 @@ $(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
$(call ARCHIVE, $(BIN), $(OBJS))
@touch .built
.context:
@ -88,16 +90,17 @@ $(COBJS): %$(OBJEXT): %.c
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@$(MKDEP) $(ROOTDEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

23
apps/fixedwing_att_control/fixedwing_att_control_att.c
View File

@ -112,8 +112,9 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
}
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
struct vehicle_rates_setpoint_s *rates_sp)
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp)
{
static int counter = 0;
static bool initialized = false;
@ -125,8 +126,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
static PID_t pitch_controller;
if(!initialized)
{
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
@ -145,12 +145,21 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
/* Pitch (P) */
float pitch_sp_rollcompensation = att_sp->pitch_body + p.pitch_roll_compensation_p * att_sp->roll_body;
rates_sp->pitch = pid_calculate(&pitch_controller, pitch_sp_rollcompensation, att->pitch, 0, 0);
/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
/* Yaw (from coordinated turn constraint or lateral force) */
//TODO
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
counter++;

6
apps/fixedwing_att_control/fixedwing_att_control_att.h
View File

@ -41,9 +41,11 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att,
struct vehicle_rates_setpoint_s *rates_sp);
const struct vehicle_attitude_s *att,
const float speed_body[],
struct vehicle_rates_setpoint_s *rates_sp);
#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */

278
apps/fixedwing_att_control/fixedwing_att_control_main.c
View File

@ -58,40 +58,16 @@
#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 <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <fixedwing_att_control_rate.h>
#include <fixedwing_att_control_att.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* Prototypes */
/**
* Deamon management function.
@ -117,118 +93,211 @@ static int deamon_task; /**< Handle of deamon task / thread */
int fixedwing_att_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */
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));
/* 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));
/* 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 = { .fd = att_sub, .events = POLLIN };
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* Check if there is a new position measurement or attitude setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool att_sp_updated;
orb_check(att_sp_sub, &att_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (att_sp_updated)
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_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;
printf("FW ATT CONTROL: Did not get a valid R\n");
}
}
/* welcome user */
printf("[fixedwing att_control] started\n");
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* declare and safely initialize all structs */
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 manual_control_setpoint_s manual_sp;
memset(&manual_sp, 0, sizeof(manual_sp));
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
// static struct debug_key_value_s debug_output;
// memset(&debug_output, 0, sizeof(debug_output));
/* control */
/* output structs */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* 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 debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1';
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
/* subscribe */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
while(!thread_should_exit)
{
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* 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;
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
} else {
att_sp.thrust = 0.0f;
}
/* Control */
att_sp.yaw_body = 0;
if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
/* Attitude Control */
fixedwing_att_control_attitude(&att_sp,
&att,
&rates_sp);
// XXX disable yaw control, loiter
/* Attitude Rate Control */
} 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 */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX
actuators.control[3] = 0.7f;
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
/* 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[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
}
/* publish output */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
printf("[fixedwing_att_control] exiting, stopping all motors.\n");
thread_running = false;
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
/* 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);
}
}
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
printf("[fixedwing_att_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);
close(att_sub);
close(actuator_pub);
close(att_sub);
close(actuator_pub);
close(rates_pub);
fflush(stdout);
exit(0);
fflush(stdout);
exit(0);
return 0;
return 0;
}
@ -239,6 +308,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
exit(1);
}
@ -268,7 +338,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
2048,
fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
@ -283,9 +353,11 @@ int fixedwing_att_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_att_control is running\n");
} else {
printf("\tfixedwing_att_control not started\n");
}
exit(0);
}

72
apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,6 +34,8 @@
/**
* @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#include <fixedwing_att_control_rate.h>
@ -59,9 +61,33 @@
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller
PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller
PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
//Yaw control parameters //XXX TODO this is copy paste, asign correct values
PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec
/* feedforward compensation */
PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */
struct fw_rate_control_params {
float rollrate_p;
@ -73,7 +99,7 @@ struct fw_rate_control_params {
float yawrate_p;
float yawrate_i;
float yawrate_awu;
float pitch_thr_ff;
};
struct fw_rate_control_param_handles {
@ -86,7 +112,7 @@ struct fw_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_awu;
param_t pitch_thr_ff;
};
@ -98,17 +124,18 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
static int parameters_init(struct fw_rate_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing
h->rollrate_i = param_find("FW_ROLLR_I");
h->rollrate_awu = param_find("FW_ROLLR_AWU");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_p = param_find("FW_PITCHR_P");
h->pitchrate_i = param_find("FW_PITCHR_I");
h->pitchrate_awu = param_find("FW_PITCHR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
h->yawrate_p = param_find("FW_YAWR_P");
h->yawrate_i = param_find("FW_YAWR_I");
h->yawrate_awu = param_find("FW_YAWR_AWU");
h->pitch_thr_ff = param_find("FW_PITCH_THR_P");
return OK;
}
@ -124,14 +151,14 @@ static int parameters_update(const struct fw_rate_control_param_handles *h, stru
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
return OK;
}
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators)
const float rates[],
struct actuator_controls_s *actuators)
{
static int counter = 0;
static bool initialized = false;
@ -147,8 +174,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if(!initialized)
{
if (!initialized) {
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
@ -167,12 +193,14 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
}
/* Roll Rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer...
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
/* pitch rate (PI) */
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
/* set pitch minus feedforward throttle compensation (nose pitches up from throttle */
actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust;
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
counter++;

4
apps/fixedwing_att_control/fixedwing_att_control_rate.h
View File

@ -42,7 +42,7 @@
#include <uORB/topics/actuator_controls.h>
int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[],
struct actuator_controls_s *actuators);
const float rates[],
struct actuator_controls_s *actuators);
#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */

566
apps/fixedwing_control/fixedwing_control.c
View File

@ -1,566 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
* Modifications: Doug Weibel <douglas.weibel@colorado.edu>
*
* 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 fixedwing_control.c
* Implementation of a fixed wing attitude and position controller.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/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 <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/debug_key_value.h>
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.
*/
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/*
* Controller parameters, accessible via MAVLink
*
*/
// Roll control parameters
PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f); // Roll rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
//Pitch control parameters
PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
// Need to add functionality to suppress integrator windup while on the ground
// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f); // Pitch rate limit in radians/sec
PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
struct fw_att_control_params {
float rollrate_p;
float rollrate_i;
float rollrate_awu;
float rollrate_lim;
float roll_p;
float roll_lim;
float pitchrate_p;
float pitchrate_i;
float pitchrate_awu;
float pitchrate_lim;
float pitch_p;
float pitch_lim;
};
struct fw_att_control_param_handles {
param_t rollrate_p;
param_t rollrate_i;
param_t rollrate_awu;
param_t rollrate_lim;
param_t roll_p;
param_t roll_lim;
param_t pitchrate_p;
param_t pitchrate_i;
param_t pitchrate_awu;
param_t pitchrate_lim;
param_t pitch_p;
param_t pitch_lim;
};
// TO_DO - Navigation control will be moved to a separate app
// Attitude control will just handle the inner angle and rate loops
// to control pitch and roll, and turn coordination via rudder and
// possibly throttle compensation for battery voltage sag.
PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
struct fw_pos_control_params {
float heading_p;
float heading_lim;
};
struct fw_pos_control_param_handles {
param_t heading_p;
param_t heading_lim;
};
/**
* Initialize all parameter handles and values
*
*/
static int att_parameters_init(struct fw_att_control_param_handles *h);
/**
* Update all parameters
*
*/
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p);
/**
* Initialize all parameter handles and values
*
*/
static int pos_parameters_init(struct fw_pos_control_param_handles *h);
/**
* Update all parameters
*
*/
static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
/**
* The fixed wing control main thread.
*
* The main loop executes continously and calculates the control
* response.
*
* @param argc number of arguments
* @param argv argument array
*
* @return 0
*
*/
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 */
printf("[fixedwing control] started\n");
/* output structs */
struct actuator_controls_s actuators;
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* 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 att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* Subscribe to global position, attitude and rc */
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* subscribe to attitude, motor setpoints and system state */
struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* Mainloop setup */
unsigned int loopcounter = 0;
uint64_t last_run = 0;
uint64_t last_run_pos = 0;
bool global_sp_updated_set_once = false;
struct fw_att_control_params p;
struct fw_att_control_param_handles h;
struct fw_pos_control_params ppos;
struct fw_pos_control_param_handles hpos;
/* initialize the pid controllers */
att_parameters_init(&h);
att_parameters_update(&h, &p);
pos_parameters_init(&hpos);
pos_parameters_update(&hpos, &ppos);
// TO_DO Fix output limit functionallity of PID controller or add that function elsewhere
PID_t roll_rate_controller;
pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t roll_angle_controller;
pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
p.roll_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_rate_controller;
pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
PID_t pitch_angle_controller;
pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
p.pitch_lim,PID_MODE_DERIVATIV_NONE);
PID_t heading_controller;
pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
100.0f,PID_MODE_DERIVATIV_SET); // Temporary arbitrarily large limit
// XXX remove in production
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
// This is the top of the main loop
while(!thread_should_exit) {
struct pollfd fds[1] = {
{ .fd = att_sub, .events = POLLIN },
};
int ret = poll(fds, 1, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* XXX this means no sensor data - should be critical or emergency */
printf("[fixedwing control] WARNING: Not getting attitude - estimator running?\n");
} else {
// FIXME SUBSCRIBE
if (loopcounter % 100 == 0) {
att_parameters_update(&h, &p);
pos_parameters_update(&hpos, &ppos);
pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f,
p.rollrate_awu, p.rollrate_lim);
pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f,
0.0f, p.roll_lim);
pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f,
p.pitchrate_awu, p.pitchrate_lim);
pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f,
0.0f, p.pitch_lim);
pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f \n",
//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
}
/* if position updated, run position control loop */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
if (global_sp_updated) {
global_sp_updated_set_once = true;
}
/* checking has to happen before the read, as the read clears the changed flag */
/* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
//orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
// XXX update to switch between external attitude reference and the
// attitude calculated here
char name[10];
if (pos_updated) {
/* get position */
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (global_sp_updated_set_once) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
/* calculate delta T */
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/* calculate bearing error */
float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d,
global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d);
/* shift error to prevent wrapping issues */
float bearing_error = target_bearing - att.yaw;
if (loopcounter % 2 == 0) {
sprintf(name, "hdng err1");
memcpy(dbg.key, name, sizeof(name));
dbg.value = bearing_error;
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
}
if (bearing_error < M_PI_F) {
bearing_error += 2.0f * M_PI_F;
}
if (bearing_error > M_PI_F) {
bearing_error -= 2.0f * M_PI_F;
}
if (loopcounter % 2 != 0) {
sprintf(name, "hdng err2");
memcpy(dbg.key, name, sizeof(name));
dbg.value = bearing_error;
orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
}
/* calculate roll setpoint, do this artificially around zero */
att_sp.roll_body = pid_calculate(&heading_controller, bearing_error,
0.0f, att.yawspeed, deltaT);
/* limit roll angle output */
if (att_sp.roll_body > ppos.heading_lim) {
att_sp.roll_body = ppos.heading_lim;
heading_controller.saturated = 1;
}
if (att_sp.roll_body < -ppos.heading_lim) {
att_sp.roll_body = -ppos.heading_lim;
heading_controller.saturated = 1;
}
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
} else {
/* no setpoint, maintain level flight */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
}
att_sp.thrust = 0.7f;
}
/* calculate delta T */
const float deltaTpos = (hrt_absolute_time() - last_run_pos) / 1000000.0f;
last_run_pos = hrt_absolute_time();
if (verbose && (loopcounter % 20 == 0)) {
printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
}
// actuator control[0] is aileron (or elevon roll control)
// Commanded roll rate from P controller on roll angle
float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
att.roll, 0.0f, deltaTpos);
// actuator control from PI controller on roll rate
actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command,
att.rollspeed, 0.0f, deltaTpos);
// actuator control[1] is elevator (or elevon pitch control)
// Commanded pitch rate from P controller on pitch angle
float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body,
att.pitch, 0.0f, deltaTpos);
// actuator control from PI controller on pitch rate
actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command,
att.pitchspeed, 0.0f, deltaTpos);
// actuator control[3] is throttle
actuators.control[3] = att_sp.thrust;
/* publish attitude setpoint (for MAVLink) */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* publish actuator setpoints (for mixer) */
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
loopcounter++;
}
}
printf("[fixedwing_control] exiting.\n");
thread_running = false;
return 0;
}
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_control {start|stop|status}\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 fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
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("\tfixedwing_control is running\n");
} else {
printf("\tfixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int att_parameters_init(struct fw_att_control_param_handles *h)
{
/* PID parameters */
h->rollrate_p = param_find("FW_ROLLRATE_P");
h->rollrate_i = param_find("FW_ROLLRATE_I");
h->rollrate_awu = param_find("FW_ROLLRATE_AWU");
h->rollrate_lim = param_find("FW_ROLLRATE_LIM");
h->roll_p = param_find("FW_ROLL_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitchrate_p = param_find("FW_PITCHRATE_P");
h->pitchrate_i = param_find("FW_PITCHRATE_I");
h->pitchrate_awu = param_find("FW_PITCHRATE_AWU");
h->pitchrate_lim = param_find("FW_PITCHRATE_LIM");
h->pitch_p = param_find("FW_PITCH_P");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
{
param_get(h->rollrate_p, &(p->rollrate_p));
param_get(h->rollrate_i, &(p->rollrate_i));
param_get(h->rollrate_awu, &(p->rollrate_awu));
param_get(h->rollrate_lim, &(p->rollrate_lim));
param_get(h->roll_p, &(p->roll_p));
param_get(h->roll_lim, &(p->roll_lim));
param_get(h->pitchrate_p, &(p->pitchrate_p));
param_get(h->pitchrate_i, &(p->pitchrate_i));
param_get(h->pitchrate_awu, &(p->pitchrate_awu));
param_get(h->pitchrate_lim, &(p->pitchrate_lim));
param_get(h->pitch_p, &(p->pitch_p));
param_get(h->pitch_lim, &(p->pitch_lim));
return OK;
}
static int pos_parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->heading_p = param_find("FW_HEADING_P");
h->heading_lim = param_find("FW_HEADING_LIM");
return OK;
}
static int pos_parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->heading_p, &(p->heading_p));
param_get(h->heading_lim, &(p->heading_lim));
return OK;
}

378
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
View File

@ -57,24 +57,31 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.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>
/*
* Controller parameters, accessible via MAVLink
*
*/
PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track
PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians
PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */
struct fw_pos_control_params {
float heading_p;
float headingr_p;
float headingr_i;
float headingr_lim;
float xtrack_p;
float altitude_p;
float roll_lim;
@ -83,11 +90,13 @@ struct fw_pos_control_params {
struct fw_pos_control_param_handles {
param_t heading_p;
param_t headingr_p;
param_t headingr_i;
param_t headingr_lim;
param_t xtrack_p;
param_t altitude_p;
param_t roll_lim;
param_t pitch_lim;
};
@ -99,8 +108,8 @@ struct planned_path_segments_s {
double end_lon;
float radius; // Radius of arc
float arc_start_bearing; // Bearing from center to start of arc
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
float arc_sweep; // Angle (radians) swept out by arc around center.
// Positive for clockwise, negative for counter-clockwise
};
@ -136,12 +145,14 @@ static int deamon_task; /**< Handle of deamon task / thread */
static int parameters_init(struct fw_pos_control_param_handles *h)
{
/* PID parameters */
h->heading_p = param_find("FW_HEADING_P");
h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
h->heading_p = param_find("FW_HEAD_P");
h->headingr_p = param_find("FW_HEADR_P");
h->headingr_i = param_find("FW_HEADR_I");
h->headingr_lim = param_find("FW_HEADR_LIM");
h->xtrack_p = param_find("FW_XTRACK_P");
h->altitude_p = param_find("FW_ALT_P");
h->roll_lim = param_find("FW_ROLL_LIM");
h->pitch_lim = param_find("FW_PITCH_LIM");
return OK;
}
@ -149,6 +160,9 @@ static int parameters_init(struct fw_pos_control_param_handles *h)
static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
{
param_get(h->heading_p, &(p->heading_p));
param_get(h->headingr_p, &(p->headingr_p));
param_get(h->headingr_i, &(p->headingr_i));
param_get(h->headingr_lim, &(p->headingr_lim));
param_get(h->xtrack_p, &(p->xtrack_p));
param_get(h->altitude_p, &(p->altitude_p));
param_get(h->roll_lim, &(p->roll_lim));
@ -162,172 +176,241 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
int fixedwing_pos_control_thread_main(int argc, char *argv[])
{
/* read arguments */
bool verbose = false;
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing att_control] started\n");
/* welcome user */
printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint;
memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct vehicle_global_position_s start_pos; // Temporary variable, replace with
memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available
struct vehicle_global_position_setpoint_s global_setpoint;
memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* publish attitude setpoint */
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint;
memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* publish attitude setpoint */
attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* Setup of loop */
struct pollfd fds = { .fd = att_sub, .events = POLLIN };
bool global_sp_updated_set_once = false;
/* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
float psi_track = 0.0f;
/* Setup of loop */
struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false;
while(!thread_should_exit)
{
/* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500);
float psi_track = 0.0f;
static int counter = 0;
static bool initialized = false;
int counter = 0;
static struct fw_pos_control_params p;
static struct fw_pos_control_param_handles h;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t altitude_controller;
PID_t heading_controller;
PID_t heading_rate_controller;
PID_t offtrack_controller;
PID_t altitude_controller;
if(!initialized)
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
initialized = true;
}
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while (!thread_should_exit) {
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
}
/* Check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* Load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if(pos_updated)
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
if(global_sp_updated) {
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* Control */
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
/* Simple Horizontal Control */
if(global_sp_updated_set_once)
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
if(!(distance_res != OK || xtrack_err.past_end)) {
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
}
else {
if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("next wp direction: %0.4f\n", (double)psi_track);
}
/* Simple Horizontal Control */
if (global_sp_updated_set_once) {
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* wrap difference back onto -pi..pi range */
psi_e = _wrap_pi(psi_e);
if (verbose) {
printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
printf("delta_psi_c %.4f ", (double)delta_psi_c);
printf("psi_c %.4f ", (double)psi_c);
printf("att.yaw %.4f ", (double)att.yaw);
printf("psi_e %.4f ", (double)psi_e);
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
/* limit turn rate */
if (psi_rate_c > p.headingr_lim) {
psi_rate_c = p.headingr_lim;
} else if (psi_rate_c < -p.headingr_lim) {
psi_rate_c = -p.headingr_lim;
}
float psi_rate_e = psi_rate_c - att.yawspeed;
// XXX sanity check: Assume 10 m/s stall speed and no stall condition
float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
if (ground_speed < 10.0f) {
ground_speed = 10.0f;
}
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
}
if (verbose && counter % 100 == 0)
printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
} else {
if (verbose && counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
/* Very simple Altitude Control */
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
counter++;
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
/* Very simple Altitude Control */
if(global_sp_updated_set_once && pos_updated)
{
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
/*Publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
counter++;
}
}
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
printf("[fixedwing_pos_control] exiting.\n");
thread_running = false;
close(attitude_setpoint_pub);
close(attitude_setpoint_pub);
fflush(stdout);
exit(0);
fflush(stdout);
exit(0);
return 0;
return 0;
}
@ -338,6 +421,7 @@ usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
exit(1);
}
@ -367,7 +451,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4096,
2048,
fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
@ -382,9 +466,11 @@ int fixedwing_pos_control_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_pos_control is running\n");
} else {
printf("\tfixedwing_pos_control not started\n");
}
exit(0);
}

5
apps/gps/gps.c
View File

@ -107,8 +107,8 @@ enum GPS_MODES {
#define AUTO_DETECTION_COUNT 8
const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400};
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
const int autodetection_baudrates[] = {B38400, B9600, B38400, B9600, B38400, B9600, B38400, B9600};
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_UBX, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
/****************************************************************************
* Private functions
@ -368,7 +368,6 @@ int gps_thread_main(int argc, char *argv[]) {
args.thread_should_exit_ptr = &thread_should_exit;
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args);
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
pthread_attr_t ubx_wd_attr;
pthread_attr_init(&ubx_wd_attr);

315
apps/gps/ubx.c
View File

@ -52,7 +52,7 @@
#define UBX_HEALTH_FAIL_COUNTER_LIMIT 3
#define UBX_HEALTH_PROBE_COUNTER_LIMIT 4
#define UBX_BUFFER_SIZE 1000
#define UBX_BUFFER_SIZE 500
extern bool gps_mode_try_all;
extern bool gps_mode_success;
@ -63,18 +63,10 @@ extern int current_gps_speed;
pthread_mutex_t *ubx_mutex;
gps_bin_ubx_state_t *ubx_state;
enum UBX_CONFIG_STATE ubx_config_state;
static struct vehicle_gps_position_s *ubx_gps;
//Definitions for ubx, last two bytes are checksum which is calculated below
uint8_t UBX_CONFIG_MESSAGE_PRT[] = {0xB5 , 0x62 , 0x06 , 0x00 , 0x14 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0xD0 , 0x08 , 0x00 , 0x00 , 0x80 , 0x25 , 0x00 , 0x00 , 0x07 , 0x00 , 0x01 , 0x00 , 0x00 , 0x00 , 0x00 , 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x02, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x21, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_DOP[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x30, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_SOL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_NAV_VELNED[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x01, 0x12, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00};
uint8_t UBX_CONFIG_MESSAGE_MSG_RXM_SVSI[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0x02, 0x20, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00};
void ubx_decode_init(void)
{
@ -100,15 +92,15 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b)
int ubx_parse(uint8_t b, char *gps_rx_buffer)
{
// printf("b=%x\n",b);
//printf("b=%x\n",b);
if (ubx_state->decode_state == UBX_DECODE_UNINIT) {
if (b == 0xb5) {
if (b == UBX_SYNC_1) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC1;
}
} else if (ubx_state->decode_state == UBX_DECODE_GOT_SYNC1) {
if (b == 0x62) {
if (b == UBX_SYNC_2) {
ubx_state->decode_state = UBX_DECODE_GOT_SYNC2;
} else {
@ -122,16 +114,25 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
//check for known class
switch (b) {
case UBX_CLASS_NAV: //NAV
case UBX_CLASS_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = ACK;
break;
case UBX_CLASS_NAV:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = NAV;
break;
case UBX_CLASS_RXM: //RXM
case UBX_CLASS_RXM:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = RXM;
break;
case UBX_CLASS_CFG:
ubx_state->decode_state = UBX_DECODE_GOT_CLASS;
ubx_state->message_class = CFG;
break;
default: //unknown class: reset state machine
ubx_decode_init();
break;
@ -193,9 +194,36 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_decode_init();
break;
}
break;
case CFG:
switch (b) {
case UBX_MESSAGE_CFG_NAV5:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = CFG_NAV5;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
case ACK:
switch (b) {
case UBX_MESSAGE_ACK_ACK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_ACK;
break;
case UBX_MESSAGE_ACK_NAK:
ubx_state->decode_state = UBX_DECODE_GOT_MESSAGEID;
ubx_state->message_id = ACK_NAK;
break;
default: //unknown class: reset state machine, should not happen
ubx_decode_init();
break;
}
break;
default: //should not happen
ubx_decode_init();
break;
@ -542,6 +570,75 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
break;
}
case ACK_ACK: {
// printf("GOT ACK_ACK\n");
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
switch (ubx_config_state) {
case UBX_CONFIG_STATE_PRT:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_NAV5:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
ubx_config_state++;
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
case UBX_CONFIG_STATE_MSG_NAV_DOP:
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
case UBX_CONFIG_STATE_MSG_NAV_SOL:
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
case UBX_CONFIG_STATE_MSG_RXM_SVSI:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
ubx_config_state++;
break;
default:
break;
}
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_ACK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
case ACK_NAK: {
// printf("GOT ACK_NAK\n");
gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) gps_rx_buffer;
//Check if checksum is valid
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
if (gps_verbose) printf("[gps] the ubx gps returned: not acknowledged\n");
ret = 1;
} else {
if (gps_verbose) printf("[gps] ACK_NAK: checksum invalid\n");
ret = 0;
}
// Reset state machine to decode next packet
ubx_decode_init();
return ret;
break;
}
default: //something went wrong
ubx_decode_init();
@ -574,53 +671,105 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
message[length - 2] = ck_a;
message[length - 1] = ck_b;
// printf("[%x,%x]", ck_a, ck_b);
// printf("[%x,%x]\n", message[length-2], message[length-1]);
}
int configure_gps_ubx(int *fd)
{
//fflush(((FILE *)fd));
// only needed once like this
const type_gps_bin_cfg_prt_packet_t cfg_prt_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_PRT,
.length = UBX_CFG_PRT_LENGTH,
.portID = UBX_CFG_PRT_PAYLOAD_PORTID,
.mode = UBX_CFG_PRT_PAYLOAD_MODE,
.baudRate = current_gps_speed,
.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK,
.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK,
.ck_a = 0,
.ck_b = 0
};
//TODO: write this in a loop once it is tested
//UBX_CFG_PRT_PART:
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
// only needed once like this
const type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_NAV5,
.length = UBX_CFG_NAV5_LENGTH,
.mask = UBX_CFG_NAV5_PAYLOAD_MASK,
.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL,
.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE,
.ck_a = 0,
.ck_b = 0
};
usleep(100000);
// this message is reusable for different configuration commands, so not const
type_gps_bin_cfg_msg_packet cfg_msg_packet = {
.clsID = UBX_CLASS_CFG,
.msgID = UBX_MESSAGE_CFG_MSG,
.length = UBX_CFG_MSG_LENGTH,
.rate = UBX_CFG_MSG_PAYLOAD_RATE
};
//NAV_POSLLH:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
usleep(100000);
uint64_t time_before_config = hrt_absolute_time();
//NAV_TIMEUTC:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
usleep(100000);
while(hrt_absolute_time() < time_before_config + UBX_CONFIG_TIMEOUT) {
//NAV_DOP:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
usleep(100000);
// if (gps_verbose) printf("[gps] ubx config state: %d\n", ubx_config_state);
//NAV_SOL:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
usleep(100000);
switch (ubx_config_state) {
case UBX_CONFIG_STATE_PRT:
// if (gps_verbose) printf("[gps] Configuring ubx with baudrate: %d\n", cfg_prt_packet.baudRate);
//NAV_SVINFO:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_VELNED:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
usleep(100000);
//RXM_SVSI:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
usleep(100000);
return 0;
write_config_message_ubx((uint8_t*)(&cfg_prt_packet), sizeof(cfg_prt_packet), fd);
break;
case UBX_CONFIG_STATE_NAV5:
write_config_message_ubx((uint8_t*)(&cfg_nav5_packet), sizeof(cfg_nav5_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_DOP:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_SOL:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_MSG_RXM_SVSI:
cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
write_config_message_ubx((uint8_t*)(&cfg_msg_packet), sizeof(cfg_msg_packet), fd);
break;
case UBX_CONFIG_STATE_CONFIGURED:
if (gps_verbose) printf("[gps] ubx configuration finished\n");
return OK;
break;
default:
break;
}
usleep(10000);
}
if (gps_verbose) printf("[gps] ubx configuration timeout\n");
return ERROR;
}
@ -637,22 +786,17 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
fds.events = POLLIN;
// UBX GPS mode
// This blocks the task until there is something on the buffer
while (1) {
//check if the thread should terminate
if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread);
// printf("exiting mtk thread\n");
// fflush(stdout);
ret = 1;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c);
// printf("Read %x\n",c);
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid ubx sentence.
// Flush the buffer and note the overflow event.
@ -671,7 +815,7 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
int msg_read = ubx_parse(c, gps_rx_buffer);
if (msg_read > 0) {
// printf("Found sequence\n");
//printf("Found sequence\n");
break;
}
@ -688,28 +832,41 @@ int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
return ret;
}
int write_config_message_ubx(uint8_t *message, size_t length, int fd)
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd)
{
//calculate and write checksum to the end
uint8_t ck_a = 0;
uint8_t ck_b = 0;
unsigned int i;
for (i = 2; i < length; i++) {
uint8_t buffer[2];
ssize_t result_write = 0;
//calculate and write checksum to the end
for (i = 0; i < length-2; i++) {
ck_a = ck_a + message[i];
ck_b = ck_b + ck_a;
}
// printf("[%x,%x]\n", ck_a, ck_b);
// write sync bytes first
buffer[0] = UBX_SYNC_1;
buffer[1] = UBX_SYNC_2;
unsigned int result_write = write(fd, message, length);
result_write += write(fd, &ck_a, 1);
result_write += write(fd, &ck_b, 1);
fsync(fd);
// write config message without the checksum
result_write = write(*fd, buffer, sizeof(buffer));
result_write += write(*fd, message, length-2);
return (result_write != length + 2); //return 0 as success
buffer[0] = ck_a;
buffer[1] = ck_b;
// write the checksum
result_write += write(*fd, buffer, sizeof(buffer));
fsync(*fd);
if ((unsigned int)result_write != length + 2)
return ERROR;
return OK;
}
void *ubx_watchdog_loop(void *args)
@ -723,6 +880,9 @@ void *ubx_watchdog_loop(void *args)
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* first try to configure the GPS anyway */
configure_gps_ubx(fd);
/* GPS watchdog error message skip counter */
bool ubx_healthy = false;
@ -732,7 +892,7 @@ void *ubx_watchdog_loop(void *args)
bool once_ok = false;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
ubx_config_state = UBX_CONFIG_STATE_PRT;
//int err_skip_counter = 0;
while (!(*thread_should_exit)) {
@ -833,23 +993,6 @@ void *ubx_loop(void *args)
/* set parameters for ubx */
if (configure_gps_ubx(fd) != 0) {
printf("[gps] Configuration of gps module to ubx failed\n");
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present);
} else {
if (gps_verbose) printf("[gps] Attempting to configure GPS to UBX binary protocol\n");
// XXX Shouldn't the system status only change if the module is known to work ok?
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present_enabled);
}
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
ubx_gps = &ubx_gps_d;

117
apps/gps/ubx.h
View File

@ -49,15 +49,17 @@
//internal definitions (not depending on the ubx protocol
#define CONFIGURE_UBX_FINISHED 0
#define CONFIGURE_UBX_MESSAGE_ACKNOWLEDGED 1
#define CONFIGURE_UBX_MESSAGE_NOT_ACKNOWLEDGED 2
#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */
#define UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS 3000000 /**< Allow 3 seconds maximum inter-message time */
#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 500000 /**< Check for current state every 0.4 seconds */
#define UBX_WATCHDOG_WAIT_TIME_MICROSECONDS 1000000 /**< Check for current state every second */
#define UBX_CONFIG_TIMEOUT 500000
#define APPNAME "gps: ubx"
#define UBX_SYNC_1 0xB5
#define UBX_SYNC_2 0x62
//UBX Protocoll definitions (this is the subset of the messages that are parsed)
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_RXM 0x02
@ -72,6 +74,24 @@
#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_CFG_PRT 0x00
#define UBX_MESSAGE_CFG_NAV5 0x24
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */
#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_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< 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_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} /**< UART1 chosen */
// ************
@ -216,7 +236,7 @@ typedef type_gps_bin_rxm_svsi_packet gps_bin_rxm_svsi_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgId;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
@ -226,7 +246,7 @@ typedef type_gps_bin_ack_ack_packet gps_bin_ack_ack_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgId;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
@ -234,15 +254,91 @@ typedef struct {
typedef type_gps_bin_ack_nak_packet gps_bin_ack_nak_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t portID;
uint8_t res0;
uint16_t res1;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t pad;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_prt_packet;
typedef type_gps_bin_cfg_prt_packet type_gps_bin_cfg_prt_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet;
typedef type_gps_bin_cfg_nav5_packet type_gps_bin_cfg_nav5_packet_t;
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t rate[6];
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet;
typedef type_gps_bin_cfg_msg_packet type_gps_bin_cfg_msg_packet_t;
// END the structures of the binary packets
// ************
enum UBX_CONFIG_STATE {
UBX_CONFIG_STATE_NONE = 0,
UBX_CONFIG_STATE_PRT = 1,
UBX_CONFIG_STATE_NAV5 = 2,
UBX_CONFIG_STATE_MSG_NAV_POSLLH = 3,
UBX_CONFIG_STATE_MSG_NAV_TIMEUTC = 4,
UBX_CONFIG_STATE_MSG_NAV_DOP = 5,
UBX_CONFIG_STATE_MSG_NAV_SVINFO = 6,
UBX_CONFIG_STATE_MSG_NAV_SOL = 7,
UBX_CONFIG_STATE_MSG_NAV_VELNED = 8,
UBX_CONFIG_STATE_MSG_RXM_SVSI = 9,
UBX_CONFIG_STATE_CONFIGURED = 10
};
enum UBX_MESSAGE_CLASSES {
CLASS_UNKNOWN = 0,
NAV = 1,
RXM = 2,
ACK = 3
ACK = 3,
CFG = 4
};
enum UBX_MESSAGE_IDS {
@ -254,7 +350,10 @@ enum UBX_MESSAGE_IDS {
NAV_DOP = 4,
NAV_SVINFO = 5,
NAV_VELNED = 6,
RXM_SVSI = 7
RXM_SVSI = 7,
CFG_NAV5 = 8,
ACK_ACK = 9,
ACK_NAK = 10
};
enum UBX_DECODE_STATES {
@ -304,7 +403,7 @@ int configure_gps_ubx(int *fd);
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
int write_config_message_ubx(uint8_t *message, size_t length, int fd);
int write_config_message_ubx(const uint8_t *message, const size_t length, const int *fd);
void calculate_ubx_checksum(uint8_t *message, uint8_t length);

4
apps/interpreters/Kconfig
View File

@ -7,7 +7,7 @@ comment "Interpreters"
source "$APPSDIR/interpreters/ficl/Kconfig"
config PCODE
config INTERPRETERS_PCODE
bool "Pascal p-code interpreter"
default n
---help---
@ -16,6 +16,6 @@ config PCODE
configuration implies that you have performed the required installation of the
Pascal run-time code.
if PCODE
if INTERPRETERS_PCODE
endif

4
apps/interpreters/Make.defs
View File

@ -34,10 +34,10 @@
#
############################################################################
ifeq ($(CONFIG_PCODE),y)
ifeq ($(CONFIG_INTERPRETERS_PCODE),y)
CONFIGURED_APPS += interpreters/pcode
endif
ifeq ($(CONFIG_FICL),y)
ifeq ($(CONFIG_INTERPRETERS_FICL),y)
CONFIGURED_APPS += interpreters/ficl
endif

32
apps/interpreters/Makefile
View File

@ -33,7 +33,7 @@
#
############################################################################
-include $(TOPDIR)/.config # Current configuration
-include $(TOPDIR)/.config
# Sub-directories containing interpreter runtime
@ -41,30 +41,36 @@ SUBDIRS = pcode ficl
# Create the list of installed runtime modules (INSTALLED_DIRS)
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
define ADD_DIRECTORY
INSTALLED_DIRS += ${shell if exist $1\Makefile (echo $1)}
endef
else
define ADD_DIRECTORY
INSTALLED_DIRS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi}
endef
endif
$(foreach DIR, $(SUBDIRS), $(eval $(call ADD_DIRECTORY,$(DIR))))
all: nothing
.PHONY: nothing context depend clean distclean
define SDIR_template
$(1)_$(2):
$(Q) $(MAKE) -C $(1) $(2) TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"
endef
$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),depend)))
$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),clean)))
$(foreach SDIR, $(INSTALLED_DIRS), $(eval $(call SDIR_template,$(SDIR),distclean)))
nothing:
context:
depend:
@for dir in $(INSTALLED_DIRS) ; do \
$(MAKE) -C $$dir depend TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
depend: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_depend)
clean:
@for dir in $(INSTALLED_DIRS) ; do \
$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
clean: $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_clean)
distclean: clean
@for dir in $(INSTALLED_DIRS) ; do \
$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
done
distclean: clean $(foreach SDIR, $(INSTALLED_DIRS), $(SDIR)_distclean)

4
apps/interpreters/ficl/Kconfig
View File

@ -3,7 +3,7 @@
# see misc/tools/kconfig-language.txt.
#
config FICL
config INTERPRETERS_FICL
bool "Ficl Forth interpreter"
default n
---help---
@ -11,6 +11,6 @@ config FICL
apps/interpreters/ficl directory. Use of this configuration assumes
that you have performed the required installation of the Ficl run-time code.
if FICL
if INTERPRETERS_FICL
endif

31
apps/interpreters/ficl/Makefile
View File

@ -1,7 +1,7 @@
############################################################################
# apps/interpreters/ficl/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Copyright (C) 2011-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
@ -35,14 +35,11 @@
BUILDDIR := ${shell pwd | sed -e 's/ /\\ /g'}
-include $(TOPDIR)/.config
-include $(TOPDIR)/Make.defs
include $(APPDIR)/Make.defs
# Tools
INCDIR = $(TOPDIR)/tools/incdir.sh
ifeq ($(WINTOOL),y)
INCDIROPT = -w
endif
@ -69,10 +66,14 @@ COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
ifeq ($(CONFIG_WINDOWS_NATIVE),y)
BIN = ..\..\libapps$(LIBEXT)
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
ifeq ($(WINTOOL),y)
BIN = ..\\..\\libapps$(LIBEXT)
else
BIN = ../../libapps$(LIBEXT)
endif
endif
ROOT_DEPPATH = --dep-path .
@ -95,24 +96,24 @@ debug:
@#echo "CFLAGS: $(CFLAGS)"
.built: debug $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
@touch .built
$(call ARCHIVE, $(BIN), $(OBJS))
$(Q) touch .built
context:
.depend: debug Makefile $(SRCS)
@$(MKDEP) $(ROOT_DEPPATH) $(SRC_DEPPATH) $(FICL_DEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
$(Q) $(MKDEP) $(ROOT_DEPPATH) $(SRC_DEPPATH) $(FICL_DEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep
$(Q) touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call DELFILE, .context)
$(call DELFILE, .built)
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
$(call DELFILE, Make.dep)
$(call DELFILE, .depend)
-include Make.dep

159
apps/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c Normal file
View File

@ -0,0 +1,159 @@
/* ----------------------------------------------------------------------
* 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
*/

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